Linear Quadratic Regulator in 3 Ways

Slide: Explore the Linear Quadratic Regulator (LQR) through the Indirect Shooting Method (Pontryagin's Principle), the Optimization Approach (Quadratic Programming), and the Recursive Solution (Riccati Equation). Inspired by Zachary Manchester's Spring 2024-25 lecture "Optimal Control and Reinforcement Learning".

Introduction

Problem Formulation

Consider a discrete-time linear system:

$$x_{n+1}=A_n{x}_n+B_n{u}_n$$

Quadratic cost function:

$$\underset{x_{1:N},u_{1:N-1}}{min}J=\sum _{n=1}^{N-1}\underset{\text{running cost}}{\underbrace{[\frac{1}{2}{x}_n^{\mathrm{\top }}{Q}_n{x}_n+\frac{1}{2}{u}_n^{\mathrm{\top }}{R}_n{u}_n]}}+\underset{\text{terminal cost}}{\underbrace{\frac{1}{2}{x}_N^{\mathrm{\top }}{Q}_N{x}_N}}$$

Assumptions:

• $(A_n,B_n)$ is controllable and $(A_n,C_n)$ is observable
• $Q_n⪰0,R_n⪰0,Q_N⪰0$

Indirect Shooting: PMP Perspective

Problem Formulation and Optimality Conditions

Consider the deterministic discrete-time optimal control problem:

$$\begin{array}{rl}\underset{x_{1:N},u_{1:N-1}}{min}& \sum _{n=1}^{N-1}l(x_n,u_n)+l_F(x_N)\\ \text{s.t. }& x_{n+1}=f(x_n,u_n)\\ & u_n\in \mathcal{U}\end{array}$$

The first-order necessary conditions for optimality can be derived using:

• The Lagrangian framework (special case of KKT conditions)
• Pontryagin's Minimum Principle (PMP)

Lagrangian Formulation

Form the Lagrangian:

$$L=\sum _{n=1}^{N-1}l(x_n,u_n)+{\lambda }_{n+1}^{\mathrm{\top }}(f(x_n,u_n)-x_{n+1})+l_F(x_N)$$

Define the Hamiltonian:

$$H(x_n,u_n,{\lambda }_{n+1})=l(x_n,u_n)+{\lambda }_{n+1}^{\mathrm{\top }}f(x_n,u_n)$$

Rewrite the Lagrangian using the Hamiltonian:

$$L=H(x_1,u_1,{\lambda }_2)+[\sum _{n=2}^{N-1}H(x_n,u_n,{\lambda }_{n+1})-{\lambda }_n^{\mathrm{\top }}{x}_n]+l_F(x_N)-{\lambda }_N^{\mathrm{\top }}{x}_N$$

Optimality Conditions

Take derivatives with respect to $x$ and $\lambda$:

$$\begin{array}{rl}\frac{\partial L}{\partial {\lambda }_n}& =\frac{\partial H}{\partial {\lambda }_n}-x_{n+1}=f(x_n,u_n)-x_{n+1}=0\\ \frac{\partial L}{\partial x_n}& =\frac{\partial H}{\partial x_n}-{\lambda }_n^{\mathrm{\top }}=\frac{\partial l}{\partial x_n}+{\lambda }_{n+1}^{\mathrm{\top }}\frac{\partial f}{\partial x_n}-{\lambda }_n^{\mathrm{\top }}=0\\ \frac{\partial L}{\partial x_N}& =\frac{\partial l_F}{\partial x_N}-{\lambda }_N^{\mathrm{\top }}=0\end{array}$$

For $u$, we write the minimization explicitly to handle constraints:

$$\begin{array}{rl}{u}_n=& \arg \underset{u}{min}H(x_n,u,{\lambda }_{n+1})\\ & \text{s.t. }u\in \mathcal{U}\end{array}$$

Summary of Necessary Conditions

The first-order necessary conditions can be summarized as:

$$\begin{array}{rl}{x}_{n+1}& ={\mathrm{\nabla }}_{\lambda }H(x_n,u_n,{\lambda }_{n+1})\\ {\lambda }_n& ={\mathrm{\nabla }}_{x}H(x_n,u_n,{\lambda }_{n+1})\\ u_n& =\arg \underset{u}{min}H(x_n,u,{\lambda }_{n+1}),\text{s.t. }u\in \mathcal{U}\\ {\lambda }_N& =\frac{\partial l_F}{\partial x_N}\end{array}$$

In continuous time, these become:

$$\begin{array}{rl}\dot{x}& ={\mathrm{\nabla }}_{\lambda }H(x,u,\lambda )\\ -\dot{\lambda }& ={\mathrm{\nabla }}_{x}H(x,u,\lambda )\\ u& =\arg \underset{\tilde{u}}{min}H(x,\tilde{u},\lambda ),\text{s.t. }\tilde{u}\in \mathcal{U}\\ \lambda (t_F)& =\frac{\partial l_F}{\partial x}\end{array}$$

Application to LQR Problems

For LQR problems with quadratic cost and linear dynamics:

$$\begin{array}{rl}l(x_n,u_n)& =\frac{1}{2}(x_n^{\mathrm{\top }}{Q}_n{x}_n+u_n^{\mathrm{\top }}{R}_n{u}_n)\\ l_F(x_N)& =\frac{1}{2}{x}_N^{\mathrm{\top }}{Q}_N{x}_N\\ f(x_n,u_n)& =A_n{x}_n+B_n{u}_n\end{array}$$

The necessary conditions simplify to:

$$\begin{array}{rl}{x}_{n+1}& =A_n{x}_n+B_n{u}_n\\ {\lambda }_n& =Q_n{x}_n+A_n^{\mathrm{\top }}{\lambda }_{n+1}\\ {\lambda }_N& =Q_N{x}_N\\ u_n& =-R_n^{-1}{B}_n^{\mathrm{\top }}{\lambda }_{n+1}\end{array}$$

This forms a linear two-point boundary value problem.

Indirect Shooting Algorithm for LQR

Procedure:

1. Make initial guess for control sequence $u_{1:N-1}$
2. Forward pass: Simulate dynamics to get state trajectory $x_{1:N}$
3. Backward pass:
   • Set terminal costate: ${\lambda }_N=Q_N{x}_N$
   • Compute costate trajectory: ${\lambda }_n=Q_n{x}_n+A_n^{\mathrm{\top }}{\lambda }_{n+1}$
   • Compute control adjustment: $\mathrm{\Delta }{u}_n=-R_n^{-1}{B}_n^{\mathrm{\top }}{\lambda }_{n+1}-u_n$
4. Line search: Update controls $u_n\leftarrow u_n+\alpha \mathrm{\Delta }{u}_n$
5. Iterate until convergence

Direct Approach: QP Perspective

LQR as Quadratic Programming Problem

Assume $x_1$ is given, define the decision variable vector and the block-diagonal matrix:

$$z=\left[\begin{array}{c}{u}_1\\ x_2\\ u_2\\ ⋮\\ x_N\end{array}\right],H=\left[\begin{array}{c}{R}_1\\ & Q_2\\ & & R_2\\ & & & \ddots \\ & & & & Q_N\end{array}\right]$$

The dynamics constraints can be expressed as

$$\underset{C}{\underbrace{\left[\begin{array}{cc}{B}_1& -I\\ & A_2& B_2& -I\\ & & & \ddots \\ & & & A_{N-1}& B_{N-1}& -I\end{array}\right]}}\left[\begin{array}{c}{u}_1\\ x_2\\ ⋮\\ x_N\end{array}\right]=\underset{d}{\underbrace{\left[\begin{array}{c}-A_1{x}_1\\ 0\\ ⋮\\ 0\end{array}\right]}}$$

QP Formulation and KKT Conditions

The LQR problem becomes the QP:

$$\underset{z}{min}J=\frac{1}{2}{z}^{\mathrm{\top }}Hz\text{subject to}Cz=d$$

The Lagrangian of this QP is:

$$\mathcal{L}(z,\lambda )=\frac{1}{2}{z}^{\mathrm{\top }}Hz+{\lambda }^{\mathrm{\top }}(Cz-d)$$

The KKT conditions are:

$$\begin{array}{rl}{\mathrm{\nabla }}_z\mathcal{L}& =Hz+C^{\mathrm{\top }}\lambda =0\\ {\mathrm{\nabla }}_{\lambda }\mathcal{L}& =Cz-d=0\end{array}$$

This leads to the linear system:

$$\left[\begin{array}{cc}H& C^{\mathrm{\top }}\\ C& 0\end{array}\right]\left[\begin{array}{c}z\\ \lambda \end{array}\right]=\left[\begin{array}{c}0\\ d\end{array}\right]$$

We get the exact solution by solving one linear system!

Riccati Equation Solution

KKT System Structure for LQR

The KKT system for LQR has a highly structured sparse form, consider an $N=4$ case:

$$\left[\begin{array}{ccccccc}{R}_1& & & & & & B_1^T\\ & Q_2& & & & & I& A_2^T\\ & & R_2& & & & & B_2^T\\ & & & Q_3& & & -I& A_3^T\\ & & & & R_3& & & B_3^T\\ & & & & & Q_4& & -I\\ B_1& -I& & & & & \\ & A_2& B_2& -I& & & \\ & & A_3& B_3& -I& & \end{array}\right]\left[\begin{array}{c}{u}_1\\ x_2\\ u_2\\ x_3\\ u_3\\ x_4\\ {\lambda }_2\\ {\lambda }_3\\ {\lambda }_4\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ 0\\ 0\\ -A_1{x}_1\\ 0\\ 0\end{array}\right]$$

Deriving the Riccati Recursion

Start from the terminal condition (blue equation):

$$Q_4{x}_4-{\lambda }_4=0\Rightarrow {\lambda }_4=Q_4{x}_4$$

Move to the previous equation (red equation):

$$R_3{u}_3+B_3^{\mathrm{\top }}{\lambda }_4=R_3{u}_3+B_3^{\mathrm{\top }}{Q}_4{x}_4=0$$

Substitute $x_4=A_3{x}_3+B_3{u}_3$:

$$R_3{u}_3+B_3^{\mathrm{\top }}{Q}_4(A_3{x}_3+B_3{u}_3)=0$$

Solve for $u_3$:

$$u_3=-\underset{K_3}{\underbrace{(R_3+B_3^{\mathrm{\top }}{Q}_4{B}_3{)}^{-1}{B}_3^{\mathrm{\top }}{Q}_4{A}_3}}{x}_3$$

Deriving the Riccati Recursion (Cont'd)

Now consider the green equation:

$$Q_3{x}_3-{\lambda }_3+A_3^{\mathrm{\top }}{\lambda }_4=0$$

Substitute ${\lambda }_4=Q_4{x}_4$ and $x_4=A_3{x}_3+B_3{u}_3$:

$$Q_3{x}_3-{\lambda }_3+A_3^{\mathrm{\top }}{Q}_4(A_3{x}_3+B_3{u}_3)=0$$

Substitute $u_3=-K_3{x}_3$:

$$Q_3{x}_3-{\lambda }_3+A_3^{\mathrm{\top }}{Q}_4(A_3{x}_3-B_3{K}_3{x}_3)=0$$

Solve for ${\lambda }_3$:

$${\lambda }_3=\underset{P_3}{\underbrace{(Q_3+A_3^{\mathrm{\top }}{Q}_4(A_3-B_3{K}_3))}}{x}_3$$

Riccati Recursion Formula

We now have a recursive relationship. Generalizing:

$$\begin{array}{rl}{P}_N& =Q_N\\ K_k& =(R_k+B_k^{\mathrm{\top }}{P}_{k+1}{B}_k{)}^{-1}{B}_k^{\mathrm{\top }}{P}_{k+1}{A}_k\\ P_k& =Q_k+A_k^{\mathrm{\top }}{P}_{k+1}(A_k-B_k{K}_k)\end{array}$$

This is the celebrated Riccati equation.

The solution process involves:

1. A backward Riccati pass to compute $P_k$ and $K_k$ for $k=N-1,\dots ,1$
2. A forward rollout to compute $x_{1:N}$ and $u_{1:N-1}$ using $u_k=-K_k{x}_k$

Computational Complexity

Naive QP Solution: Treats problem as one big least-squares.

• Computational cost: $\mathbf{O}\mathbf{[}{\mathbf{N}}^{\mathbf{3}}\mathbf{(}\mathbf{n}\mathbf{+}\mathbf{m}{\mathbf{)}}^{\mathbf{3}}\mathbf{]}$
• Must be re-solved from scratch for any change.

Riccati Recursion: Exploits the temporal structure.

• Computational cost: $\mathbf{O}\mathbf{[}\mathbf{N}\mathbf{(}\mathbf{n}\mathbf{+}\mathbf{m}{\mathbf{)}}^{\mathbf{3}}\mathbf{]}$
• Exponentially faster for long horizons (large $N$).

The Riccati Solution is More Than Just Fast:

• It provides a ready-to-use feedback policy: ${\mathbf{u}}_{\mathbf{k}}\mathbf{=}\mathbf{-}{\mathbf{K}}_{\mathbf{k}}{\mathbf{x}}_{\mathbf{k}}$
• This policy is adaptive: optimal for any initial state $x_1$, not just a single one.
• It enables real-time control by naturally rejecting disturbances.
• And it delivers the exact same optimal solution as the QP.

Conclusion

Summary

Finite-Horizon Problems

• Use Riccati recursion backward in time
• Store gain matrices $K_n$
• Apply time-varying feedback

Infinite-Horizon Problems

• Solve algebraic Riccati equation offline
• Use constant gain matrix $K_{\mathrm{\infty }}$
• Implement simple state feedback
• Algebraic Riccati Equation (ARE):$$P_{\mathrm{\infty }}=Q+A^{\mathrm{\top }}{P}_{\mathrm{\infty }}A-A^{\mathrm{\top }}{P}_{\mathrm{\infty }}B(R+B^{\mathrm{\top }}{P}_{\mathrm{\infty }}B{)}^{-1}{B}^{\mathrm{\top }}{P}_{\mathrm{\infty }}A$$