\documentclass[dvips,landscape]{foils}
\usepackage{graphicx,psfrag}
\special{! TeXDict begin /landplus90{true}store end }

\input defs.tex

\addtolength{\textheight}{2.5cm} % lieven had 3cm

\begin{document}

\input header.tex
\setcounter{lecture}{3}  
\lecturefl{Continuous time linear quadratic regulator}

\vfill 
\BIT
\item continuous-time LQR problem
\item dynamic programming solution
\item Hamiltonian system and two point boundary value problem
\item infinite horizon LQR
\item direct solution of ARE via Hamiltonian
\EIT
\vfill

\oursectionfl{Continuous-time LQR problem}

continuous-time system $\dot x = Ax+Bu$, $x(0)=x_0$
\vfill

problem: choose $u: \reals_+ \rightarrow \reals^m$ to minimize
\[
J =
\int_0^T  \left( x(\tau)^T Qx(\tau) +
u(\tau)^T R u(\tau) \right) \; d\tau + x(T)^T Q_fx(T)
\]

\BIT\itemsep -4pt
\item $T$ is \emph{time horizon}
\item $Q=Q^T \geq 0$, $Q_f=Q_f^T \geq 0$, $R = R^T > 0$
are \emph{state cost}, \emph{final state cost},
and \emph{input cost} matrices
\EIT

\vfill

\ldots an \emph{infinite-dimensional problem}:
(trajectory $u: [0,T] \rightarrow \reals^m$ is variable)

\oursectionfl{Dynamic programming solution}

we'll solve LQR problem using dynamic programming

for $0 \leq t \leq T$ we define the \textbf{value function} 
$V_t: \reals^n  \rightarrow \reals$ by
\[
V_t(z) = \min_{u}
\int_t^T x(\tau)^T Qx(\tau) +
u(\tau)^T R u(\tau) \; d\tau + x(T)^T Q_fx(T)
\]
subject to $x(t)=z$, $\dot x=Ax+Bu$

\vfill

\BIT\itemsep -3pt
\item minimum is taken over all possible signals $u:[t,T]\rightarrow 
\reals^m$
\item $V_t (z)$ gives the minimum LQR cost-to-go, starting
from state $z$ at time $t$
\item $V_T(z)=z^TQ_fz$
\EIT

\newpage
\mbox{ }

\vfill

\textbf{fact:} $V_t$ is quadratic,
\ie, $V_t(z)=z^TP_t z$, where $P_t=P_t^T \geq 0$

\vfill

similar to discrete-time case:
\BIT
\item $P_t$ can be found from a \emph{differential equation}
running backward in time from $t=T$
\item the LQR optimal $u$ is easily expressed in terms of $P_t$
\EIT

\vfill

\newpage
we start with $x(t)=z$

\vfill

let's take $u(t)=w \in \reals^m$, a constant, over the
time interval $[t,t+h]$, where $h>0$ is small

\vfill

cost incurred over $[t,t+h]$ is
\[
\int_t^{t+h} x(\tau)^TQx(\tau) + w^TRw \; d\tau \approx
h (z^TQz + w^TRw)
\]

\vfill

and we end up at
$x(t+h) \approx z+ h (Az+Bw)$

\vfill

\newpage
min-cost-to-go from where we land is approximately
\BEAS
\lefteqn{V_{t+h}(z+h(Az+Bw))}\\
&=&(z+h(Az+Bw))^TP_{t+h} (z+h(Az+Bw))\\
&\approx& 
(z+h(Az+Bw))^T (P_t+ h \dot P_t) (z+h(Az+Bw))\\
&\approx& z^TP_tz + h\left( (Az+Bw)^T P_t z + z^T P_t(Az+Bw) 
+z^T\dot P_tz \right)
\EEAS
(dropping $h^2$ and higher terms)

cost incurred plus min-cost-to-go is approximately
\[
z^TP_tz + h\left( z^TQz+w^TRw+ (Az+Bw)^T P_t z+ z^TP_t(Az+Bw) 
+z^T\dot P_tz \right)
\]

minimize over $w$ to get (approximately) optimal $w$:
\[
2hw^TR + 2hz^TP_tB =0
\]
so 
\[
w^* = -R^{-1}B^T P_t z
\]

thus optimal $u$ is time-varying linear state feedback:
\[
u_{\rm lqr}(t) = K_t x(t), \qquad
K_t = -R^{-1}B^T P_t
\]

\oursectionfl{HJ equation}

now let's substitute $w^*$ into HJ equation:
\[
\begin{array}{l}
z^TP_tz \approx z^TP_tz +\\
+ h\left( z^TQz+w^{*T}Rw^*+ (Az+Bw^*)^T P_t z+ z^T P_t(Az+Bw^*) 
+z^T\dot P_tz \right)
\end{array}
\]

yields, after simplification,
\[
%(A-BR^{-1}B^TP_t)^T P_t + P_t (A-BR^{-1}B^TP_t) + \dot P_t +
%Q+ P_tBR^{-1}B^TP_t = 0
-\dot P_t = A^TP_t+P_tA - P_tBR^{-1}B^TP_t+Q
\]
which is the \emph{Riccati differential equation} for the 
LQR problem

we can solve it (numerically) using the \emph{final condition}
$P_T=Q_f$

\newpage

\oursectionfl{Summary of cts-time LQR solution via DP}

1. solve Riccati differential equation
\[
-\dot P_t = A^TP_t+P_tA - P_tBR^{-1}B^TP_t+Q, \qquad
P_T=Q_f
\]
(backward in time)

2. optimal $u$ is $u_{\rm lqr}(t)=K_t x(t)$, $K_t := -R^{-1} B^TP_t$

\vfill
DP method readily extends to time-varying $A$, $B$, $Q$, $R$, and 
tracking problem

\oursectionfl{Steady-state regulator}

\vfill
usually $P_t$ rapidly converges as $t$ decreases below $T$
\vfill
limit $P_{\rm ss}$ satisfies (cts-time)
algebraic Riccati equation (ARE)
\[
A^TP+PA - PBR^{-1}B^TP+Q =0
\]
a quadratic matrix equation

\vfill

\BIT\itemsep -4pt
\item $P_{\rm ss}$ can be found 
by (numerically) integrating the Riccati differential
equation, or by direct methods
\item
for $t$ not close to horizon $T$, LQR optimal input is 
approximately a linear, constant state feedback 
\[
u(t)=K_{\rm ss}x(t), \qquad
K_{\rm ss}= -R^{-1} B^TP_{\rm ss}
\]
\EIT

\oursectionfl{Derivation via discretization}

let's discretize using
small step size $h>0$, with $Nh=T$
\[
x((k+1)h) \approx x(kh)+h \dot x(kh) = (I+hA) x(kh)+h Bu (kh)
\]
\[
J \approx \frac{h}{2}\sum_{k=0}^{N-1}
\left(x(kh)^TQx(kh)+u(kh)^TRu(kh) \right)
+\frac{1}{2} x(Nh)^TQ_f x(Nh)
\]

this yields a discrete-time LQR problem, with parameters
\[
\tilde A = I+hA, \qquad
\tilde B = hB, \qquad
\tilde Q = hQ, \qquad
\tilde R = hR, \qquad
\tilde Q_f = Q_f
\]

\newpage
solution to discrete-time LQR problem is $u(kh)=\tilde K_k x(kh)$,
\[
\tilde K_k = 
- (\tilde R+\tilde B^T\tilde P_{k+1}\tilde B)^{-1}\tilde B^T \tilde
P_{k+1} \tilde A
\]
\[
\tilde P_{k-1} = \tilde Q +\tilde A^T\tilde P_k \tilde A
-\tilde A^T\tilde P_k \tilde B
(\tilde R+ \tilde B^T\tilde P_k \tilde B)^{-1}
\tilde B^T \tilde P_k \tilde A 
\]

substituting and keeping only $h^0$ and $h^1$ terms yields
\[
\tilde P_{k-1} = hQ +\tilde P_k + hA^T\tilde P_k 
+ h \tilde P_k A - h \tilde P_kBR^{-1}B^T \tilde P_k
\]
which is the same as
\[
-\frac{1}{h} (\tilde P_k-\tilde P_{k-1}) = Q+A^T\tilde P_k+\tilde P_kA
-\tilde P_kBR^{-1}B^T\tilde P_k
\]

letting $h \rightarrow 0$ we see that $\tilde P_k \rightarrow
P_{kh}$, where
\[
-\dot P =  Q+A^TP+PA -PBR^{-1}B^TP
\]

\newpage
similarly, we have
\BEAS
\tilde K_k &=& 
- (\tilde R+\tilde B^T\tilde P_{k+1}\tilde B)^{-1}\tilde B^T \tilde
P_{k+1} \tilde A\\
&=& - (hR+h^2B^T\tilde P_{k+1}B)^{-1}hB^T \tilde P_{k+1} (I+hA)\\
& \rightarrow &
-R^{-1}B^T P_{kh}
\EEAS
as $h \rightarrow 0$

\oursectionfl{Derivation using Lagrange multipliers}

pose as constrained problem:
\[
\begin{array}{ll}
\mbox{minimize} & J = \frac{1}{2} \int_0^T  
x(\tau)^TQx(\tau)+u(\tau)^TRu(\tau) \; d\tau +
\frac{1}{2} x(T)^TQ_fx(T)\\[5pt]
\mbox{subject to} & \dot x(t) = Ax(t)+Bu(t), \quad t \in [0,T]
\end{array}
\]

\BIT\itemsep -4pt
\item optimization variable is \emph{function}
$u:[0,T]\rightarrow \reals^m$
\item infinite number of equality constraints, one for each
$t \in [0,T]$
\EIT

introduce Lagrange multiplier \emph{function}
$\lambda : [0,T] \rightarrow \reals^n$ and form %Lagrangian
\[
L = J + \int_0^T 
\lambda(\tau)^T(Ax(\tau)+Bu(\tau)-\dot x(\tau)) \; d\tau
\]

\oursectionfl{Optimality conditions}

{\small (note: you need \emph{distribution theory} to really make 
sense of the derivatives here \ldots)}

\vfill

from $\nabla_{u(t)} L = Ru(t) + B^T\lambda (t)=0$ we get
$u(t) = -R^{-1}B^T\lambda(t)$

to find $\nabla _{x(t)} L$, we use
\[
\int_0^T \lambda(\tau)^T\dot x(\tau) \; d\tau =
\lambda(T)^Tx(T)-\lambda(0)^Tx(0)
-\int_0^T \dot \lambda(\tau)^T x(\tau) \; d\tau 
\]

from $\nabla_{x(t)} L = Qx(t)+A^T\lambda(t) +\dot \lambda(t) =0$
we get
\[
\dot \lambda (t) = -A^T\lambda(t)-Qx(t)
\]

from $\nabla_{x(T)} L = Q_fx(T) -\lambda(T)=0$, we get
$\lambda(T)=Q_fx(T)$

\oursectionfl{Co-state equations}

optimality conditions are
\[
\dot x =Ax+Bu, \quad x(0)=x_0, \qquad
\dot \lambda  = -A^T \lambda -Qx, \quad \lambda (T)= Q_fx(T)
\]

using $u(t) =-R^{-1}B^T \lambda(t)$, can write as
\[
\frac{d}{dt} 
\left[\begin{array}{c} x(t)\\ \lambda (t) \end{array}\right]
= 
\left[\begin{array}{cc}
A & -BR^{-1}B^T \\ -Q & -A^T
\end{array}\right]
\left[\begin{array}{c} x(t)\\ \lambda (t) \end{array}\right]
\]

\BIT \itemsep -4pt
\item $2n \times 2n$ matrix above is called \emph{Hamiltonian} 
for problem
\item with conditions $x(0)=x_0$, $\lambda (T)=Q_fx(T)$, called
\emph{two-point boundary value problem}
\EIT

\newpage

as in discrete-time case, we can show that $\lambda(t) = P_tx(t)$,
where 
\[
-\dot P_t = A^TP_t+P_tA - P_tBR^{-1}B^TP_t+Q, \qquad
P_T = Q_f
\]

in other words, value function $P_t$ gives simple relation 
between $x$ and $\lambda$

to show this, we show that $\lambda=Px$ satisfies
co-state equation $\dot \lambda =-A^T\lambda-Qx$

\BEAS
\dot \lambda  &=& \frac{d}{dt} (Px)
= \dot P x + P \dot x\\
&=& -(Q+A^TP+PA-PBR^{-1}B^TP)x + P(Ax-BR^{-1}B^T\lambda)\\
&=& -Qx-A^TPx+PBR^{-1}B^TPx -PBR^{-1}B^TPx\\
&=& -Qx-A^T\lambda
\EEAS


\oursectionfl{Solving Riccati differential equation via Hamiltonian}

the (quadratic) Riccati differential equation
\[
- \dot P = A^TP+PA - PBR^{-1}B^TP+Q
\]
and the (linear) Hamiltonian differential equation
\[
\frac{d}{dt} 
\left[\begin{array}{c} x(t)\\ \lambda (t) \end{array}\right]
= 
\left[\begin{array}{cc}
A & -BR^{-1}B^T \\ -Q & -A^T
\end{array}\right]
\left[\begin{array}{c} x(t)\\ \lambda (t) \end{array}\right]
\]
are closely related

$\lambda (t) = P_t x(t)$ suggests that $P$ should have the form
$P_t = \lambda (t) x(t)^{-1}$\\
(but this doesn't make sense unless $x$ and $\lambda$ are scalars)

\newpage
consider the Hamiltonian \emph{matrix} (linear) differential equation
\[
\frac{d}{dt} 
\left[\begin{array}{c} X(t)\\ Y(t) \end{array}\right] = 
\left[\begin{array}{cc}
A & -BR^{-1}B^T \\ -Q & -A^T
\end{array}\right]
\left[\begin{array}{c} X(t)\\ Y(t) \end{array}\right]
\]
where $X(t),~Y(t) \in \reals^{n \times n}$

then, $Z(t)=Y(t)X(t)^{-1}$ satisfies Riccati differential equation
\[
- \dot Z = A^TZ+ZA - ZBR^{-1}B^TZ+Q
\]

hence we can solve Riccati DE by solving (linear) matrix Hamiltonian
DE, with final conditions $X(T)=I$, $Y(T)=Q_f$, and forming 
$P(t) = Y(t)X(t)^{-1}$

\newpage
\BEAS
\dot Z &=& \frac{d}{dt} YX^{-1} \\
&=& \dot Y X^{-1} -YX^{-1}\dot X X^{-1}\\
&=& (-QX-A^TY)X^{-1} -YX^{-1}\left(AX-BR^{-1}B^TY\right) X^{-1}\\
&=& -Q-A^TZ -ZA+ZBR^{-1}B^TZ
\EEAS

where we use two identities:
\BIT
\item
$\displaystyle \frac{d}{dt} \left(F(t)G(t)\right) 
= \dot F(t) G(t) + F(t)\dot G(t)$
\item
$\displaystyle
\frac{d}{dt}\left( F(t)^{-1} \right) = - F(t)^{-1} \dot F(t) F(t)^{-1}
$
\EIT

\oursectionfl{Infinite horizon LQR}

we now consider the infinite horizon cost function
\[
J =  \int_0^\infty  x(\tau)^T Qx(\tau) +
u(\tau)^T R u(\tau) \; d\tau
\]

we define the value function as
\[
V(z) = \min_{u}
\int_0^\infty x(\tau)^T Qx(\tau) +
u(\tau)^T R u(\tau) \; d\tau 
\]
subject to $x(0)=z$, $\dot x=Ax+Bu$

we assume that $(A,B)$ is controllable, so $V$ is finite for all $z$

can show that $V$ is quadratic: $V(z)=z^TPz$, where $P=P^T \geq 0$

\newpage

optimal $u$ is $u(t)=Kx(t)$, where $K=-R^{-1}B^TP$\\
(\ie, a constant linear state feedback)

HJ equation is ARE 
\[
Q+A^TP+PA-PBR^{-1}B^TP =0
\]
which together with $P \geq 0$ characterizes $P$

can solve as limiting value of Riccati DE, or via direct method

\oursectionfl{Closed-loop system}

with $K$ LQR optimal state feedback gain, closed-loop system is
\[
\dot x = Ax+Bu = (A+BK)x
\]

\textbf{fact:} closed-loop system is stable when $(Q,A)$ observable 
and $(A,B)$ controllable

we denote eigenvalues of $A+BK$, called
\emph{closed-loop eigenvalues}, as
$\lambda_1, \ldots, \lambda_n$

with assumptions above, $\Re \lambda_i < 0$

\oursectionfl{Solving ARE via Hamiltonian}

\[
\left[\begin{array}{cc}
A & -BR^{-1}B^T \\ -Q & -A^T
\end{array}\right]
\left[\begin{array}{c} I \\ P \end{array}\right]
=
\left[\begin{array}{c} A-BR^{-1}B^TP \\ -Q-A^TP \end{array}\right]
=
\left[\begin{array}{c} A+BK\\ -Q-A^TP \end{array}\right]
\]
and so
\[
\left[\begin{array}{cc}
I & 0 \\ -P & I
\end{array}\right]
\left[\begin{array}{cc}
A & -BR^{-1}B^T \\ -Q & -A^T
\end{array}\right]
\left[\begin{array}{cc} I & 0\\ P & I \end{array}\right]
=
\left[\begin{array}{cc} A+BK & -BR^{-1}B^T \\ 0 & -(A+BK)^T
\end{array}\right]
\]
where $0$ in lower left corner comes from ARE

note that
\[
\left[\begin{array}{cc} I & 0\\ P & I \end{array}\right]^{-1} =
\left[\begin{array}{cc} I & 0\\ -P & I \end{array}\right]
\]

\newpage
we see that:
\BIT
\item eigenvalues of Hamiltonian $H$
are $\lambda_1, \ldots, \lambda_n$ 
and $-\lambda_1, \ldots, -\lambda_n$
\item hence, closed-loop eigenvalues are the eigenvalues of $H$ with
negative real part
\EIT

\newpage
let's assume $A+BK$ is diagonalizable, \ie,
\[
T^{-1}(A+BK)T=\Lambda = \diag(\lambda_1, \ldots, \lambda_n)
\]
then we have
$T^T(-A-BK)^T T^{-T}= -\Lambda$, so
\BEAS
\lefteqn{\left[\begin{array}{cc}
T^{-1} & 0 \\ 0 & T^T
\end{array}\right]
\left[\begin{array}{cc} A+BK & -BR^{-1}B^T \\ 0 & -(A+BK)^T
\end{array}\right]
\left[\begin{array}{cc}
T & 0 \\ 0 & T^{-T}
\end{array}\right]}\\
&=&
\left[\begin{array}{cc} \Lambda & -T^{-1}BR^{-1}B^T T^{-T}\\
0 & -\Lambda \end{array}\right]
\EEAS

\newpage
putting it together we get
\BEAS
\lefteqn{\left[\begin{array}{cc} T^{-1} & 0 \\ 0 & T^T \end{array}\right]
\left[\begin{array}{cc} I & 0 \\ -P & I \end{array}\right]
H
\left[\begin{array}{cc} I & 0 \\ P & I \end{array}\right]
\left[\begin{array}{cc} T & 0 \\ 0 & T^{-T} \end{array}\right]}\\
&=&
\left[\begin{array}{cc} T^{-1} & 0 \\ -T^TP & T^T \end{array}\right]
H
\left[\begin{array}{cc} T & 0 \\ PT & T^{-T} \end{array}\right]\\
&=&
\left[\begin{array}{cc} \Lambda & -T^{-1}BR^{-1}B^T T^{-T}\\
0 & -\Lambda \end{array}\right]
\EEAS
and so
\[
H
\left[\begin{array}{c} T \\ PT \end{array}\right] =
\left[\begin{array}{c} T \\ PT \end{array}\right]
\Lambda
\]

thus, the $n$ columns of 
$\displaystyle \left[\begin{array}{c} T \\ PT \end{array}\right]$
are the eigenvectors of $H$ associated with the stable
eigenvalues $\lambda_1, \ldots, \lambda_n$

\oursectionfl{Solving ARE via Hamiltonian}

\BIT
\item find eigenvalues of $H$, and let $\lambda_1, \ldots, \lambda_n$
denote the $n$ stable ones\\
(there are exactly $n$ stable and $n$ unstable ones)
\item find associated eigenvectors $v_1, \ldots, v_n$, and partition
as
\[
\left[\begin{array}{ccc} v_1 & \cdots & v_n \end{array}\right] =
\left[\begin{array}{c} X \\ Y \end{array}\right] \in
\reals^{2n \times n}
\]
\item
$P=YX^{-1}$ is unique PSD solution of the ARE
\EIT

(this is very close to the method used in practice, which does not
require $A+BK$ to be diagonalizable)
\end{document}
