ELEC 5650 - Linear Quadratic Regulator

"We have decided to call the entire field of control and communication theory, whether in the machine or in the animal, by the name Cybernetics, which we form from the Greek ... for steersman."

 -- by Norbert Wiener

This is the lecture notes for 'ELEC 5650: Networked Sensing, Estimation and Control' in the 2024-25 Spring semester, delivered by Prof. Ling Shi at HKUST. In this session, we will cover Linear Quadratic Regulator (LQR) theory and its applications in control systems.

1. Mathematic Tools
2. Estimation Theory
3. Kalman Filter
4. Linear Quadratic Regulator <--

Linear Quadratic Regulator

Dynamic Programming

Consider a discrete-time dynamical system over a finite horizon $N$. The goal is to find a control policy that minimizes the expected cumulative cost:

$$J^{\pi }(x_0)={\mathbb{E}}_{{\omega }_k}\{g_N(x_N)+\sum _{k=0}^{N-1}{g}_k[x_k,{\mu }_k(x_k),{\omega }_k]\}$$

The system evolves according to:

$$x_{k+1}=f_k(x_k,u_k,{\omega }_k)$$

Principle of Optimality

"An optimal policy has the property that whatever the initial state and initial decision are, the remaining decisions must constitute an optimal policy with regard to the state resulting from the first decision."

 -- Richard Bellman

Optimal principle allows us to break down the multi-stage optimization problem into a sequence of simpler single-stage problems.

Let ${\pi }^{\ast }=\{{\mu }_0^{\ast },\cdots ,{\mu }_{N-1}^{\ast }\}$ be an optimal policy. Then for any $k$ and reachable state $x_k$, the sub-policy ${\pi }_{k\to N-1}^{\ast }=\{{\mu }_k^{\ast },\cdots ,{\mu }_{N-1}^{\ast }\}$ must minimize the cost-to-go:

$$J_{k\to N}(x_k)={\mathbb{E}}_{{\omega }_k}\{g_N(x_N)+\sum _{i=k}^{N-1}{g}_i[x_i,{\mu }_i(x_i),{\omega }_i]\}$$

This implies

$$J_k(x_k)=\underset{u_k}{min}{\mathbb{E}}_{{\omega }_k}\{g_k[x_k,{\mu }_k(x_k),{\omega }_k]+J_{k+1}f[x_k,{\mu }_k(x_k),{\omega }_k]\}$$

Dynamic Programming Algorithm

The solution is computed recursively through the following steps:

Terminal Cost

$$J_N(x_N)=g_N(x_N)$$

Backward Recursion

$$J_k(x_k)=\underset{u_k\in \mathcal{U}}{min}{\mathbb{E}}_{{\omega }_k}\{g_k(x_k,u_k,{\omega }_k)+J_{k+1}[f(x_k,u_k,{\omega }_k)]\}$$$${\mu }_k^{\ast }(x_k)=\arg \underset{u_k\in \mathcal{U}}{min}{\mathbb{E}}_{{\omega }_k}\{g_k(x_k,u_k,{\omega }_k)+J_{k+1}[f(x_k,u_k,{\omega }_k)]\}$$

Consider the following linear system

$$x_{k+1}=A{x}_k+B{u}_k$$

We wants to find a series of $u_0,\cdots ,u_{N-1}$ to minimize

$$J=\underset{g_N(x_N)}{\underbrace{x_N^{T}Q{x}_N}}+\sum _{k=0}^{N-1}(\underset{g_k(x_k,u_k,{\omega }_k)}{\underbrace{x_k^{T}Q{x}_k+u_k^{T}R{u}_k}}),Q⪰0,R\succ 0$$

Solution

Terminal Cost

$$J_N(x_N)=g_N(x_N)=x_N^{T}Q{x}_N\triangleq x_N^T{P}_N{x}_N$$

Backward Recursion

$$\begin{array}{rl}{J}_{N-1}(x_{N-1})& =\underset{u_{N-1}}{min}\{x_{N-1}^{T}Q{x}_{N-1}+u_{N-1}^{T}R{u}_{N-1}+J_N(x_N)\}\\ & =\underset{u_{N-1}}{min}\{x_{N-1}^{T}Q{x}_{N-1}+u_{N-1}^{T}R{u}_{N-1}+x_N^T{P}_N{x}_N\}\\ & =\underset{u_{N-1}}{min}\{x_{N-1}^{T}Q{x}_{N-1}+u_{N-1}^{T}R{u}_{N-1}+(A{x}_{N-1}+B{u}_{N-1}{)}^T{P}_N(A{x}_{N-1}+B{u}_{N-1})\}\\ & =\underset{u_{N-1}}{min}\{u_{N-1}^T(R+B^T{P}_{N}B)u_{N-1}+2x_{N-1}^T{A}^T{P}_{N}B{u}_{N-1}+x_{N-1}^T(Q+A^T{P}_{N}A)x_{N-1}\}\end{array}$$$$u_{N-1}^{\ast }=\underset{L_{N-1}}{\underbrace{-(R+B^T{P}_{N}B{)}^{-1}{B}^T{P}_{N}A}}{x}_{N-1}$$$$\begin{array}{rl}{J}_{N-1}(x_{N-1})& =x_{N-1}^T{P}_{N-1}{x}_{N-1}\\ & =u_{N-1}^T(R+B^T{P}_{N}B)u_{N-1}+2x_{N-1}^T{A}^T{P}_{N}B{u}_{N-1}+x_{N-1}^T(Q+A^T{P}_{N}A)x_{N-1}\\ & =x_{N-1}^T[A^T{P}_N^{T}B(R+B^T{P}_{N}B{)}^{-1}{B}^T{P}_{N}A-2A^T{P}_{N}B(R+B^T{P}_{N}B{)}^{-1}{B}^T{P}_{N}A+Q+A^T{P}_{N}A]x_{N-1}\\ & =x_{N-1}^T[Q+A^T{P}_{N}A-A^T{P}_N^{T}B(R+B^T{P}_{N}B{)}^{-1}{B}^T{P}_{N}A]x_{N-1}\end{array}$$$$P_{N-1}=Q+A^T{P}_{N}A-A^T{P}_N^{T}B(R+B^T{P}_{N}B{)}^{-1}{B}^T{P}_{N}A$$

Summary

$$P_N=Q,\{\begin{array}{l}{u}_k^{\ast }=L_k{x}_k\\ L_k=-(B^T{P}_{k+1}B+R{)}^{-1}{B}^T{P}_{k+1}A\\ P_k=A^T{P}_{k+1}A+Q-A^T{P}_{k+1}B(B^T{P}_{k+1}B+R{)}^{-1}{B}^T{P}_{k+1}A\end{array}$$

Riccati Equation

Define

$$P_{k+1}=h(P_k)=A^T{P}_{k}A+Q-A^T{P}_{k}B(B^T{P}_{k}B+R{)}^{-1}{B}^T{P}_{k}A$$

Assume $(A,B)$ is controllable, $(A,\sqrt{Q})$ is observable, then the following holds

1. $\mathrm{\exists }P\succ 0,\mathrm{\forall }{P}_0⪰0,\underset{k\to \mathrm{\infty }}{lim}{P}_k=P$
2. $P$ is the unique solution to $P=h(P)$
3. $D=A+BL$ is stable, where $L=-(B^{T}PB+R{)}^{-1}{B}^{T}PA$

Existence

We firstly prove $\mathrm{\exists }P\succ 0,P=h(P)$

Assume $P_0=0$, then $P_k=h(P_{k-1})=h^{k-1}(P_1)=h^k(P_0)=h(0)⪰0$. So for any control sequence

$$\underset{i=0}{\overset{k-1}{min}}\underset{x_0^T{g}^k(0)x_0}{\underbrace{(x_i^{T}Q{x}_i+u_i^{T}R{u}_i)}}\le \underset{i=0}{\overset{k}{min}}\underset{x_0^T{g}^{k+1}(0)x_0}{\underbrace{(x_i^{T}Q{x}_i+u_i^{T}R{u}_i)}}$$

If $P_0=0$, then $\mathrm{\forall }X⪰Y,h(X)⪰h(Y)$. For any specific control sequence ${\overline{u}}_0,\cdots {\overline{u}}_k$, there exist an associated cost ${\overline{J}}_k=\sum _{i=0}^{k-1}(x_i^{T}Q{x}_i+{\overline{u}}_i^{T}R{u}_i)$ woule be a constant.

$$\mathrm{\forall }k,x_0^T{P}_k{x}_0=x_0^T{h}^k(P_0)x_0=x_0^T{h}^k(0)x_0\le {\overline{J}}_k$$

So $P_k$ converges when $P_0=0$

Stability

$$\begin{array}{rl}P& =h(P)\\ & =A^T{P}_{k}A+Q-A^T{P}_{k}B(B^T{P}_{k}B+R{)}^{-1}{B}^T{P}_{k}A\\ & =D^{T}PD+Q+L^{T}RL\end{array}$$$$x_{k+1}=A{x}_k+B{u}_k=(A+BL)x_k=D{x}_k$$

To show $D$ is stable, we only need to show that $\mathrm{\forall }{x}_0,x_k\to 0$ as $k\to \mathrm{\infty }$

$$\{\begin{array}{l}{x}_{k+1}^{T}P{x}_{k+1}-x_k^{T}P{x}_k=x_k^T{D}^{T}PD{x}_k-x_k^{T}P{x}_k=-x_k^T(Q+L^{T}RL)x_k\\ x_k^{T}P{x}_k-x_{k-1}^{T}P{x}_{k-1}=-x_{k-1}^T(Q+L^{T}RL)x_{k-1}\\ ⋮\\ x_1^{T}P{x}_1-x_0^{T}P{x}_0=-x_0^T(Q+L^{T}RL)x_0\end{array}$$$$x_{k+1}^{T}P{x}_{k+1}=x_0^{T}P{x}_0-\sum _{i=0}^k{x}_i^T(Q+L^{T}RL)x_i$$

Because $P\ge 0$, $x_{k+1}^{T}P{x}_{k+1}⪰0$, hence

$$\sum _{i=0}^k{x}_i^T(Q+L^{T}RL)x_i\le x_0^{T}P{x}_0<\mathrm{\infty }$$

This implies

$$\underset{k\to \mathrm{\infty }}{lim}{x}_k^T(Q+L^{T}RL)x_k=0$$

While $Q+L^{T}RL\succ 0$, we must have

$$\underset{k\to \mathrm{\infty }}{lim}{x}_k=0$$

Hence, the stability is proved.

Convergence

Next we prove $\mathrm{\forall }P⪰0,P=h(P)$. Because ${\mathrm{u}}^{\ast }$ is the optimal solution, ${\overline{J}}_k\ge x_0^T{h}^k(P_0)x_0$.

$$\begin{array}{rl}{\overline{J}}_k& =x_k^T{P}_0{x}_k+\sum _{i=0}^{k-1}(x_i^{T}Q{x}_i+u_i^{T}R{u}_i)\\ & =x_0^T(D^k{)}^T{P}_0{D}^k{x}_0+\sum _{i=0}^{k-1}(x_i^T(Q+L^{T}RL)x_i)\\ & =x_0^T[(D^k{)}^T{P}_0{D}^k+\sum _{i=0}^{k-1}(D^i{)}^T(Q+L^{T}RL)D^i]x_0\\ & =x_0^T[(D^k{)}^T{P}_0{D}^k+P-(D^k{)}^{T}P{D}^k]x_0\end{array}$$$$\underset{k\to \mathrm{\infty }}{lim}{\overline{J}}_k=x_0^{T}P{x}_0$$$$\mathrm{\forall }{P}_0,\underset{k\to \mathrm{\infty }}{lim}{x}_0^T{h}^k(P_0)x_0=x_0^{T}P{x}_0$$$$\underset{k\to \mathrm{\infty }}{lim}{P}_k=\underset{k\to \mathrm{\infty }}{lim}{h}^k(P_0)=P$$

Uniqueness

Assume $\overline{P}$ is another solution to $\overline{P}=h(\overline{P})$. If $P^{\prime }\ne P$, then $\mathrm{\exists }{x}_0,x_0^T{P}^{\prime }{X}_0<x_0^{T}P{x}_0$. Contradict to the optimal feature. Hence, $P^{\prime }=P$

Linear Quadratic Gaussian

$$\begin{array}{rlr}{x}_{k+1}& =A{x}_k+B{u}_k+{\omega }_k,& {\omega }_k\sim \mathcal{N}(0,W)\\ y_k& =C{x}_k+{\nu }_k,& {\nu }_k\sim \mathcal{N}(0,V)\end{array}$$

We want to minimize the quadratic cost function:

$$J=\mathbb{E}[x_N^{T}Q{x}_N+\sum _{k=0}^{N-1}(x_k^{T}Q{x}_k+u_k^{T}R{u}_k)],Q⪰0,R\succ 0$$

By seperation principle, we can decomposed the problem to an optimal estimatior and an optimal controller.

Kalman Filter

The Kalman filter provides the optimal state estimate ${\hat{x}}_k$ with error covariance ${\mathrm{\Sigma }}_k$:

Time Update

$$\begin{array}{rl}{\hat{x}}_{k|k-1}& =A{\hat{x}}_{k-1|k-1}+B{u}_{k-1}\\ {\hat{P}}_{k|k-1}& =A{\hat{P}}_{k-1|k-1}{A}^T+W\end{array}$$

Measurement Update

$$\begin{array}{rl}{K}_k& ={\hat{P}}_{k|k-1}{C}^T(C{\hat{P}}_{k|k-1}{C}^T+V{)}^{-1}\\ {\hat{x}}_{k|k}& ={\hat{x}}_{k|k-1}+K_k(y_k-C{\hat{x}}_{k|k-1})\\ {\hat{P}}_{k|k}& =(I-K_{k}C){\hat{P}}_{k|k-1}\end{array}$$

Linear Quadratic Regulator

$$J=\sum _{k=0}^{\mathrm{\infty }}(x_k^{T}Q{x}_k+u_k^{T}R{u}_k)$$

Solve $P$ from

$$P=A^{T}PA-A^{T}PB(R+B^{T}PB{)}^{-1}{B}^{T}PA+Q$$

And solve $L$ from

$$L=-(R+B^{T}PB{)}^{-1}{B}^{T}PA$$$$u_k=L{x}_k$$