Introduction

These are notes for tutorials of the subject “Optimal and Predictive Control System” taught at the Faculty of Mechanical Engineering at the Czech Technical University in Prague.

Dating back to the 1950s optimal control is an every evolving field currently fueled by the incredible (from a historic point of view) amounts of computational power at our disposal. Although in this course we only cover the fundamentals as applied to linear system which can be successfully implemented on microcontrollers, cutting-edge approaches push hardware to its limits, allowing for the control of walking robots, autonomous vehicles, etc.

The materials are loosely divided into three blocks. We cover the basics of numerical optimization which we will become relevant for optimal control only at the very end of the course. Regardless we will attempt to provide examples that demonstrate their utility as we go. Then we will move on to the main content of the course. Starting from the quite abstract concept of the Bellman principle we cover the topic of the linear-quadratic regulator (LQR) from which we will transition to model predictive control (MPC).

We will apply each control strategy to the system of a bi-rotor, i.e. a quad-rotor simplified into the horizontal plane. As supplementary materials we will show how its dynamic model can be derived.

DISCLAIMER: These notes have not gone through a peer review process and are likely to contain (hopefully only minor) mistakes.

Controllability/Reachability

Controllability and reachability are defined for discrete-time systems as:

• A system is controllable if from any state $x_0\in {\mathbb{R}}^n$ one can achieve the state $x_N=0$ using a series of inputs $u_{0:N-1}$.
• A state $x_N\in {\mathbb{R}}^n$ is reachable if it can be achieved by applying a series of inputs $u_{0:N-1}$ when starting from the initial state $x_0=0$.

For continuous-time systems they are similarly defined as:

• A system is controllable if from any state $x(0)\in {\mathbb{R}}^n$ one can achieve the state $x(t)=0$ using a control policy $u(\tau )$, $\tau \in ⟨0,t)$.
• A state $x(t)\in {\mathbb{R}}^n$ is reachable if it can be achieved by applying a control policy $u(\tau )$, $\tau \in ⟨0,t)$ when starting from the initial state $x(0)=0$.

For linear systems all states are reachable if the system is controllable.

Controlability of LTI Discrete-Time Systems

The first $n+1$ states of a linear time-invariant system $$x_{k+1}=A{x}_k+B{u}_k,x_k\in {\mathbb{R}}^n,u_k\in {\mathbb{R}}^m,$$ assuming an initial state $x_0$ and a series of inputs $\{u_k{\}}_0^{N-1}$, can be expressed as $$\begin{array}{rl}{x}_1& =A{x}_0+B{u}_0\\ x_2& =A^2{x}_0+AB{u}_0+B{u}_1\\ & ⋮\\ x_n& =A^n{x}_0+A^{n-1}B{u}_0+A^{n-2}B{u}_1+\dots +B{u}_{n-1}\\ x_{n+1}& =A^{n+1}{x}_0+A^{n}B{u}_0+A^{n-1}B{u}_1+A^{n-2}B{u}_2+\dots +B{u}_n\end{array}$$ For square matrices that satisfy their own characteristic equation the Cayley-Hamilton theorem states that for $N\ge n$ we may express $A^N$ as a linear combination of the lower matrix powers of $A$: $$A^N=a_{0,N}I+a_{1,N}A+\dots +a_{n-1,N}{A}^{n-1}.$$ Therefore, the state $x_{n+1}$ can be rewritten as $$x_{n+1}=A^{n+1}{x}_0+A^{n-1}B(u_1+a_{n-1,n}{u}_0)+A^{n-2}B(u_2+a_{n-2,n}{u}_0)+\dots +B(u_n+a_{0,n}{u}_0)$$ which can be manipulated into the form $$x_{n+1}=A^{n+1}{x}_0+R\left[\begin{array}{c}{u}_n+a_{0,n}{u}_0\\ u_{n-1}+a_{1,n}{u}_0\\ u_1+a_{n-1,n}{u}_0\end{array}\right].$$ where $$R=\left[\begin{array}{cccc}B& AB& \dots & A^{n-1}B\end{array}\right]$$ is the controllability matrix.

The same substitution can be applied also for subsequent timesteps up-to infinity, changing only the particular form of the vector of inputs’ linear combinations. This has two consequences:

• All states reachable in $N>n$ steps are also reachable in $n$ steps (with unlimited inputs).
• If $R$ is rank deficient, some directions in the state-space cannot be effected by the inputs and therefore the system is uncontrollable.

Controlability of LTI Continuous-Time Systems

The state of a linear time-invariant system $$\dot{x}(\tau )=Ax(\tau )+Bu(\tau ),x(\tau )\in {\mathbb{R}}^n,u(\tau )\in {\mathbb{R}}^m$$ at time $t$, starting from the initial state $x(0)$ and influenced by a continuous input $u(\tau )$, can be expressed as $$x(t)=e^{At}x(0)+{\int }_0^t{e}^{A(t-\tau )}Bu(\tau )d\tau .\text{\hspace{1em}(1)}$$ For square matrices that satisfy their own characteristic equation the Cayley-Hamilton theorem has a consequence that the inifinite series $$e^{At}=I+At+\frac{(At{)}^2}{2!}+\dots$$ can be expressed using a finite number of terms $$e^{At}={\varphi }_0(t)I+{\varphi }_1(t)A+\cdots +{\varphi }_{n-1}(t)A^{n-1}.$$ After substituting it back into the second term of (1) we attain $$x(t)=e^{At}x(0)+{\int }_0^t({\varphi }_0(t-\tau )I+{\varphi }_1(t-\tau )A+\cdots +{\varphi }_{n-1}(t-\tau )A^{n-1})Bu(\tau )d\tau$$ which can be further manipulated into the form $$x(t)=e^{At}x(0)+R{\int }_0^t\left[\begin{array}{c}{\varphi }_0(t-\tau )u(\tau )\\ {\varphi }_1(t-\tau )u(\tau )\\ ⋮\\ {\varphi }_{n-1}(t-\tau )u(\tau )\end{array}\right]d\tau ,$$ where $$R=\left[\begin{array}{cccc}B& AB& \dots & A^{n-1}B\end{array}\right]$$ is the controllability matrix. If $R$ is rank deficient, some directions in the state-space cannot be effected by the inputs and therefore the system is uncontrollable.

State-Space Representation of Mechanical Systems

Equations of Motion

Let us consider manipulator equations in the form $$M(q)\ddot{q}+c(q,\dot{q})-{\tau }_g(q)=B(q)u\text{\hspace{1em}(1)}$$ who’s left-hand side may be for example derived from the system’s kinetic $T(q,\dot{q})$ and potential $V(q)$ energy expressed in generalized coordinates by expanding Lagrange equations of the second kind $$\frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{\partial L}{\partial \dot{q}}\right)-\frac{\partial L}{\partial q}=\sum _i\frac{\partial r_i}{\partial q}{F}_i,$$ where $L=T-V$. First by separating the Lagrangian into its components $$\frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{\partial T}{\partial \dot{q}}-\cancel{\frac{\partial V}{\partial \dot{q}}}\right)-\frac{\partial T}{\partial q}+\frac{\partial V}{\partial q}=\sum _i\frac{\partial r_i}{\partial q}{F}_i$$ and then applying the change rule $$\frac{{\partial }^{2}T}{\partial {\dot{q}}^2}\ddot{q}+\frac{{\partial }^{2}T}{\partial \dot{q}\partial q}\dot{q}-\frac{\partial T}{\partial q}+\frac{\partial V}{\partial q}=\sum _i\frac{\partial r_i}{\partial q}{F}_i.$$ If the external forces are linearly dependent on inputs $u_i$, such that $F_i=f_i{u}_i$, the terms of (1) as derived by this approach are $$\begin{array}{rl}M(q)& =\frac{{\partial }^{2}T}{\partial {\dot{q}}^2}\\ c(q,\dot{q})& =\frac{{\partial }^{2}T}{\partial \dot{q}\partial q}\dot{q}-\frac{\partial T}{\partial q}\\ {\tau }_g(q)& =-\frac{\partial V}{\partial q}\\ B(q)& =\left[\begin{array}{ccc}\frac{\partial r_1}{\partial q}{f}_1& \dots & \frac{\partial r_m}{\partial q}{f}_m\end{array}\right].\end{array}$$

Continuous-time dynamics

Most commonly used state-space representation of a mechanical system in continuous time is attained by concatenating $q$ and $\dot{q}$ to form the system’s state $$x=\left[\begin{array}{c}q\\ \dot{q}\end{array}\right],\dot{x}=f(x,u)=\left[\begin{array}{c}\dot{q}\\ M^{-1}(q)\left({\tau }_g(q)-c(q,\dot{q})+B(q)u\right)\end{array}\right].$$

Linearization

The system’s dynamics can be linearized by performing a Taylor expansion around a point $\{x^{\ast },u^{\ast }\}$ which yields $$\dot{x}=f(x,u)\approx f(x^{\ast },u^{\ast })+\underset{A_C}{\underbrace{\frac{\partial f}{\partial x}(x^{\ast },u^{\ast })}}\underset{\bar{x}}{\underbrace{(x-x^{\ast })}}+\underset{B_C}{\underbrace{\frac{\partial f}{\partial u}(x^{\ast },u^{\ast })}}\underset{\bar{u}}{\underbrace{(u-u^{\ast })}},$$ where also $\dot{\bar{x}}=\frac{\mathrm{d}}{\mathrm{d}t}\left(x-x^{\ast }\right)=\dot{x}$.

If $\{x^{\ast },u^{\ast }\}$ is a fixed point, i.e. $f(x^{\ast },u^{\ast })=0$, first order partial derivatives of the system’s dynamics simplify to $$\begin{array}{rl}\frac{\partial f}{\partial x}(x^{\ast },u^{\ast })& =\left[\begin{array}{cc}0& I\\ M^{-1}(q^{\ast })\left(\frac{\partial {\tau }_g}{\partial q}(q^{\ast })+\frac{\partial B}{\partial q}(q^{\ast })u^{\ast }\right)& 0\end{array}\right]\\ \frac{\partial f}{\partial u}(x^{\ast },u^{\ast })& =\left[\begin{array}{c}0\\ M^{-1}(q^{\ast })B(q^{\ast })\end{array}\right]\end{array}$$ as terms involving $\frac{\partial M^{-1}}{\partial q}$ drop out because ${\tau }_g-c+B{u}^{\ast }=0$ for all fixed points. Partial derivatives of $c$ also disappear as all of its terms contain second degree products of velocities and all velocities must be equal to zero at a fixed point.

Explicit Euler method discretization

The most basic approach with which we may discretize continuous-time dynamics of a nonlinear system is by integrating the systems state with a timestep of $h$ using the explicit Euler method $$x_{k+1}=x_k+hf(x_k,u_k).$$

Linearization

Compared to more advanced integration methods, linearization of these discrete time dynamics around a point $\{x^{\ast },u^{\ast }\}$ is trivial: $$\begin{array}{rl}\underset{{\bar{x}}_{k+1}}{\underbrace{x_{k+1}-x^{\ast }}}& =x_k+hf(x_k,u_k)-x^{\ast }\\ & \approx hf(x^{\ast },u^{\ast })+\underset{A_D}{\underbrace{\left(I+h\frac{\partial f}{\partial x}(x^{\ast },u^{\ast })\right)}}\underset{{\bar{x}}_k}{\underbrace{(x_k-x^{\ast })}}+\underset{B_D}{\underbrace{h\frac{\partial f}{\partial u}(x^{\ast },u^{\ast })}}\underset{{\bar{u}}_k}{\underbrace{(u_k-u^{\ast })}}.\end{array}$$

Cart-Pole Equations of Motion

For generalized coordinates $q={\left[\begin{array}{cc}s& \theta \end{array}\right]}^{\top }$ the system’s kinetic and potential energy are $$\begin{array}{rl}T& =\frac{1}{2}\left(m_c{\dot{s}}^2+m_p\left({\dot{x}}_p^2+{\dot{y}}_p^2\right)\right)\\ V& =-g{m}_p\cos (\theta ),\end{array}$$ where $$\begin{array}{rl}{\dot{x}}_p& =\dot{s}+l\dot{\theta }\cos (\theta )\\ {\dot{y}}_p& =l\dot{\theta }\sin (\theta ).\end{array}$$ The individual terms of the manipulator equations¹ are then $$\begin{array}{rl}M& =\left[\begin{array}{cc}{m}_c+m_p& m_{p}l\cos \left(\theta \right)\\ m_{p}l\cos \left(\theta \right)& m_p{l}^2\end{array}\right]\\ c& =\left[\begin{array}{c}-m_{p}l\sin \left(\theta \right){\dot{\theta }}^2\\ 0\end{array}\right]\\ {\tau }_g& =\left[\begin{array}{c}0\\ -g{m}_{p}l\sin \left(\theta \right)\end{array}\right]\\ B& =\left[\begin{array}{c}1\\ 0\end{array}\right],\end{array}$$ assuming a single input is acting in the direction of $s$.

Linearization in the upright configuration

From the equations we may see that for the state and input vectors $$\begin{array}{rl}x& =\left[\begin{array}{c}q\\ \dot{q}\end{array}\right]={\left[\begin{array}{cccc}s& \theta & \dot{s}& \dot{\theta }\end{array}\right]}^{\top }\\ u& =\left[\begin{array}{c}u\end{array}\right]\end{array}$$ all points $$\begin{array}{rl}{x}^{\ast }& =\left[\begin{array}{c}{q}^{\ast }\\ {\dot{q}}^{\ast }\end{array}\right]={\left[\begin{array}{cccc}s& \pi & 0& 0\end{array}\right]}^{\top },s\in \mathbb{R}\\ u^{\ast }& =\left[\begin{array}{c}0\end{array}\right]\end{array}$$ are stationary. In this configuration the relevant terms for the system’s linearization are $$\begin{array}{rl}M(q^{\ast })& =\left[\begin{array}{cc}{m}_c+m_p& m_{p}l\\ m_{p}l& m_p{l}^2\end{array}\right]\\ \frac{\partial {\tau }_g}{\partial q}(q^{\ast })& =\left[\begin{array}{cc}0& 0\\ 0& g{m}_{p}l\end{array}\right]\\ \frac{\partial B}{\partial q}(q^{\ast })& =0\\ B(q^{\ast })& =\left[\begin{array}{c}1\\ 0\end{array}\right]\end{array}$$

1. The python script for the derivation of the following terms can be found here. A very similar form of these equations of motion can also be found in section 3.2.1 of Underactuated2023. ↩

Bi-Rotor Equations of Motion

As a bi-rotor (quadcopter simplified to 2D) is essentially a rigid body, its equations of motion can be easily derived as $$\begin{array}{rl}m\ddot{x}& =-\sin (\theta )\left(u_1+u_2\right)\\ m\ddot{y}& =\cos (\theta )\left(u_1+u_2\right)-mg\\ I\ddot{\theta }& =a\left(u_1-u_2\right).\end{array}$$

Linearization in a hovering configuration

From the EoM we may see that for the state description $$\begin{array}{rlrl}x& =\left[\begin{array}{c}x\\ y\\ \theta \\ \dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right],& \dot{x}& =\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\\ -\frac{1}{m}\sin (\theta )\left(u_1+u_2\right)\\ \frac{1}{m}\cos (\theta )\left(u_1+u_2\right)-mg\\ \frac{a}{I}\left(u_1-u_2\right)\end{array}\right]\\ u& =\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right]& & \end{array}$$ all points $$\begin{array}{rl}{x}^{\ast }& ={\left[\begin{array}{cccccc}x& y& 0& 0& 0& 0\end{array}\right]}^{\top },\{x,y\}\in {\mathbb{R}}^2\\ u^{\ast }& ={\left[\begin{array}{cc}\frac{1}{2}mg& \frac{1}{2}mg\end{array}\right]}^{\top }\end{array}$$ are stationary points, for which the system’s linearization is trivial $$\begin{array}{rl}A& =\left[\begin{array}{cccccc}& & & 1& & \\ & & & & 1& \\ & & & & & 1\\ & & -g& & & \\ & & & & & \\ & & & & & \end{array}\right]\\ B& =\left[\begin{array}{cc}& \\ & \\ & \\ & \\ \frac{1}{m}& \frac{1}{m}\\ \frac{a}{I}& \frac{-a}{I}\end{array}\right]\end{array}$$

Derivation in manipulator equation form

For those who want to validate the equations by deriving the manipulator equations, kinetic and potential energy for generalized coordinates $$q=\left[\begin{array}{c}x\\ y\\ \theta \end{array}\right]$$ are $$\begin{array}{rl}T& =\frac{1}{2}m({\dot{x}}^2+{\dot{y}}^2)+\frac{1}{2}I{\dot{\theta }}^2\\ V& =mgy\end{array}$$ which should yield $$M=\left[\begin{array}{ccc}m& 0& 0\\ 0& m& 0\\ 0& 0& I\end{array}\right]c=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]{\tau }_g=\left[\begin{array}{c}0\\ -mg\\ 0\end{array}\right].$$ The manipulation matrix $$B=\left[\begin{array}{cc}-\sin (\theta )& -\sin (\theta )\\ \cos (\theta )& \cos (\theta )\\ a& -a\end{array}\right]$$ can then be derived separately by evaluating thrust vectors and differentiating moment arms.

Bi-Rotor with Payload

For generalized coordinates $q={\left[\begin{array}{cccc}y& z& \theta & \varphi \end{array}\right]}^{\top }$ the system’s kinetic and potential energy are $$\begin{array}{rl}T& =\frac{1}{2}\left(m_Q({\dot{y}}^2+{\dot{z}}^2)+I_Q{\dot{\theta }}^2+m_P({\dot{y}}_P^2+{\dot{z}}_P^2)\right)\\ V& =g{m}_{Q}z+g{m}_P(z-l\cos (\varphi ))\end{array}$$ where $${\dot{y}}_P=\dot{y}+l\dot{\varphi }\cos (\varphi ),{\dot{z}}_P=\dot{z}+l\dot{\varphi }\sin (\varphi ).$$ The two forces acting on the system and their action arms are $$F_{1,2}=u_{1,2}\left[\begin{array}{c}-\sin (\theta )\\ \cos (\theta )\end{array}\right],r_{1,2}=\left[\begin{array}{c}y\mp a\cos (\theta )\\ z\mp a\sin (\theta )\end{array}\right].$$ The individual terms of the manipulator equations are then $$\begin{array}{rl}M& =\left[\begin{array}{cccc}{m}_Q+m_P& 0& 0& l{m}_P\cos (\varphi )\\ 0& m_Q+m_P& 0& l{m}_P\sin (\varphi )\\ 0& 0& I_Q& 0\\ l{m}_P\cos (\varphi )& l{m}_P\sin (\varphi )& 0& l^2{m}_P\end{array}\right]\\ c& =\left[\begin{array}{c}-l{m}_P\sin (\varphi ){\dot{\varphi }}^2\\ l{m}_P\cos (\varphi ){\dot{\varphi }}^2\\ 0\\ 0\end{array}\right]\\ {\tau }_p& =\left[\begin{array}{c}0\\ -g(m_P+m_Q)\\ 0\\ -gl{m}_P\sin (\varphi )\end{array}\right]\\ B& =\left[\begin{array}{cc}-\sin (\theta )& -\sin (\theta )\\ \cos (\theta )& \cos (\theta )\\ -a& a\\ 0& 0\end{array}\right]\end{array}$$

Convex optimization problems

The vast majority of numerical optimization problems can be written in the form $$\begin{array}{rl}\underset{x}{\min }& f(x)\\ \text{s.t.}& g(x)\le 0\\ & h(x)=0,\end{array}$$ where $x\in {\mathbb{R}}^n$ are optimized variables, $f:{\mathbb{R}}^n\to \mathbb{R}$ is the objective function, $g:{\mathbb{R}}^n\to {\mathbb{R}}^r$ are inequality constraints and $h:{\mathbb{R}}^n\to {\mathbb{R}}^s$ are equality constraints. Here we assume that $n$, $r$, and $s$ lie in $\mathbb{N}$ and that constraints $g$ and $h$ define a non-empty set of feasible solutions.

Based on the properties of $f$, $g$, and $h$, different approaches can be taken to solve the optimization problem. For example, if the objective function is convex and the constraint are affine the problem has a unique solution (optimal value of the objective function) that can be found using gradient descend. In this course we will focus on this type of problems which are classified as convex.

We will start with a general overview of Lagrangian Duality and KKT conditions which are applicable to both convex and non-convex optimization problems. Then we will describe two specific types of convex optimization problems: linear programming (LP) and quadratic programming (QP).

Lagrangian Duality

In the following section we will derive the so-called dual problem of a (primal) optimization problem, following the derivation demonstrated in a series of short lectures which accompany the book Bierlaire2018. Possibly the most important property of the dual problem (from a practical point of view) is that it is convex even if the primal optimization problem is not Boyd2004.

Let us consider a constrained optimization problem in the form $$\begin{array}{rl}\underset{x}{\min }& f(x)\\ \text{s.t.}& g(x)\le 0\\ & h(x)=0,\end{array}$$ to which we will refer to as the primal problem. With regard to this problem, let us define the Lagrangian $$L(x,\lambda ,\mu )=f(x)+{\lambda }^{\top }h(x)+{\mu }^{\top }g(x),$$ where $\lambda$ and $\mu$ are Lagrange multipliers, and the dual function $$q(\lambda ,\mu )=\underset{x}{\min }L(x,\lambda ,\mu ).$$

Theorem

Let $x^{\ast }$ be the solution to the primal problem. If $\mu \ge 0$, then $$q(\lambda ,\mu )\le f(x^{\ast }).$$

Proof

$$q(\lambda ,\mu )=\underset{x}{\min }L(x,\lambda ,\mu )\le L(x^{\ast },\lambda ,\mu )$$

(For any $\lambda$ and $\mu \ge 0$, I can find an $x$ that minimizes the Lagrangian as well as or better than $x^{\ast }$ by violating the constraints.)

and

$$L(x^{\ast },\lambda ,\mu )=f(x^{\ast })+{\lambda }^{\top }\underset{=0}{\underbrace{h(x^{\ast })}}+\underset{\ge 0}{\underbrace{{\mu }^{\top }}}\underset{\le 0}{\underbrace{g(x^{\ast })}}\le f(x^{\ast })$$ (Because if ${\mu }_i>0$ and $g_i(x^{\ast })<0$ then ${\mu }_i{g}_i(x^{\ast })<0$)

$\square$

Corollary

Let $x^{\ast }$ be the solution to the primal problem and $x$ feasible w.r.t its constraints. If $\lambda \in {\mathbb{R}}^n$ and $\mu \in {\mathbb{R}}^p$, $\mu \ge 0$, then $$q(\lambda ,\mu )\le f(x^{\ast })\le f(x).$$

Dual problem

As was proven ${\min }_{x}L(x,\lambda ,\mu )$ provides a lower bound on the solution of the primal problem when $\mu \ge 0$. The dual problem can then be regarded as maximing this lower bound by penalizing violations of the primal problem’s constraints using the Lagrange multipliers: $$\begin{array}{rl}\underset{\lambda ,\mu }{\max }& \underset{q(\lambda ,\mu )}{\underbrace{\underset{x}{\min }L(x,\lambda ,\mu )}}\\ \text{s.t.}& \mu \ge 0\\ & (\lambda ,\mu )\in \{\lambda ,\mu \mid q(\lambda ,\mu )>-\infty \}.\end{array}$$

The second condition states that we consider on $\lambda$ and $\mu$ such that the dual function is bounded. This is necessary for the problem to be well posed.

(Weak duality) theorem

If $x^{\ast }$ is the solution to the primal problem and $({\lambda }^{\ast },{\mu }^{\ast })$ is the solution to the dual problem, then $$q({\lambda }^{\ast },{\mu }^{\ast })\le f(x^{\ast }).$$

Corollary

If one problem is unbounded the other is infeasible.

Corollary

If $\exists x^{\ast },{\lambda }^{\ast },{\mu }^{\ast }$ such that $$q({\lambda }^{\ast },{\mu }^{\ast })=f(x^{\ast }),$$ they are optimal.

Karuch-Kuhn-Tucker Conditions

The Karuch-Kuhn-Tucker (KKT) conditions are first order necessary conditions for finding the solution of an optimization problem in the form $$\begin{array}{rl}\underset{x}{\min }& f(x)\\ \text{s.t.}& g(x)\le 0\\ & h(x)=0.\end{array}$$ Furthermore, they are also sufficient conditions for convex problems, i.e. those where $f(x)$, $g(x)$ are convex and $h(x)$ is affine (linear) Boyd2004.

A solution satisfying these conditions gives us not only $x$ but also the Lagrange multipliers $\lambda$ and $\mu$ present in the Lagrangian $$L(x,\lambda ,\mu )=f(x)+{\lambda }^{\top }h(x)+{\mu }^{\top }g(x).$$

The conditions are as follows:

• stationarity $$\nabla f(x)+\nabla g(x)\mu +\nabla h(x)\lambda =0\text{\hspace{1em}(1)}$$ (The linear combination of the constraints’ gradients has to be equal to the objective function’s gradient.)

• primal feasibility $$\begin{array}{rl}g(x)& \le 0\\ h(x)& =0\end{array}\text{\hspace{1em}(2-3)}$$ (Constraints of the stated optimization problem have to be satisfied.)

• dual feasibility $$\mu \ge 0\text{\hspace{1em}(4)}$$ (For a given $x$, the columns of $\nabla g(x)$ define a polyhedral cone in which ${\mu }^{\top }\nabla g(x)$ must lie.)

• complementary slackness $${\mu }^{\top }g(x)=0\text{\hspace{1em}(5)}$$ (If $x$ lies inside the feasible set w.r.t to the condition $g(x)<0$, then $\mu =0$ and therefore ${\mu }^{\top }\nabla g(x)=0$.)

Physical interpretation

The KKT conditions can be intuitively derived though an analogy where $\nabla f(x)$ is a potential field, for example of gravity of magnetism. Let us first address only the inequality constrained case $$\begin{array}{rl}\underset{x}{\min }& f(x)\\ \text{s.t.}& g(x)\le 0,\end{array}$$ where the inequality constraints can be regarded as physical barriers. If we then recall the Lagrangian $$L(x,\lambda ,\mu )=f(x)+{\mu }^{\top }g(x),$$ the multipliers and $\mu$ can be viewed as contact forces acting against the influence of the potential field.

From this analogy we can immediately recover the dual feasibility and complementary slackness conditions (4-5) as contact forces can only be positive and do not act at a distance. The stationarity condition (1) (without the term related to the equality constraint) can then be interpreted as the contact forces and the pull of potential field being in equilibrium, which must occur at a local minima. Lastly, we are considering only solutions within the bounds of the inequality constraints, as if we were initially placed within them, satisfying the primal feasibility condition (2).

To incorporate equality constraints into this analogy we can simply reformulate them as $$0\le h(x)\le 0$$ likening them to being sandwiched between two surfaces. As we are always in contact there is no need for a complementary slackness condition and due to its bi-directional nature, elements of $\lambda$ can be both positive and negative. Therefore we must only include a primal feasibilty condition (2) and add an additional term to the stationarity condition (1).

Linear Programming

Let us consider a constrained optimization problem in the form $$\begin{array}{rl}\underset{x}{\max }& c^{\top }x\\ \text{s.t.}& Ax\le b\\ & x\ge 0.\end{array}$$

As both the objective function and constraints are linear the problem is convex. For this reason KKT conditions are not only necessary but also sufficient if a feasible $x$ w.r.t the constraints exists.

KKT conditions

The KKT conditions of this problem can be states as: $$\begin{array}{rlr}c+A^{\top }{\mu }_1-{\mu }_2& =0& \text{stationarity}\\ Ax& \le b& \text{primal feasibility}\\ x& \ge 0& \\ {\mu }_1& \ge 0& \text{dual feasibility}\\ {\mu }_2& \ge 0& \\ {\mu }_1^{\top }(Ax-b)& =0& \text{complementary slackness}\\ -{\mu }_2^{\top }x& =0& \end{array}$$

Dual problem

The dual problem of the LP problem can be written as $$\begin{array}{rl}\underset{{\mu }_1,{\mu }_2}{\max }& \underset{q({\mu }_1,{\mu }_2)}{\underbrace{\underset{x}{\min }L(x,{\mu }_1,{\mu }_2)}},L(x,{\mu }_1,{\mu }_2)=-c^{\top }x-{\mu }_1^{\top }x+{\mu }_2^{\top }(Ax-b)\\ \text{s.t.}& {\mu }_1\ge 0\\ & {\mu }_2\ge 0\\ & ({\mu }_1,{\mu }_2)\in \{{\mu }_1,{\mu }_2\mid q({\mu }_1,{\mu }_2)>-\infty \},\end{array}$$ where the Lagrangian can be manipulated into the form $$L(x,{\mu }_1,{\mu }_2)=x^{\top }(A^{\top }{\mu }_2-{\mu }_1-c)-b^{\top }{\mu }_2.$$ As we are only looking for bound solutions (last constraint) the condition $$A^{\top }{\mu }_2-{\mu }_1-c=0$$ must be satisfied and therefore $${\mu }_1=A^{\top }{\mu }_2-c$$ When substituted back into the original dual problem, after basic manipulations, we attain $$\begin{array}{rl}\underset{{\mu }_2}{\min }& b^{\top }{\mu }_2\\ \text{s.t.}& A^{\top }{\mu }_2\ge c\\ & {\mu }_2\ge 0\end{array}$$

Quadratic Programming

Let us consider a constrained optimization problem in the form $$\begin{array}{rl}\underset{x}{\min }& \frac{1}{2}{x}^{\top }Qx+c^{\top }x\\ \text{s.t.}& Ax\le b,\end{array}$$ where $Q⪰0$.

As the objective function is quadratic and the constraints linear the problem is convex. For this reason KKT conditions are not only necessary but also sufficient if a feasible $x$ w.r.t the constraints exists.

KKT Conditions

The KKT conditions of this problem can be states as: $$\begin{array}{rlr}Qx+c+A^{\top }\mu & =0& \text{stationarity}\\ Ax& \le b& \text{primal feasibility}\\ \mu & \ge 0& \text{dual feasibility}\\ {\mu }^{\top }(Ax-b)& =0& \text{complementary slackness}\end{array}$$

Dual problem

The dual problem of the QP problem can be written as $$\begin{array}{rl}\underset{\mu }{\max }& \underset{q(\mu )}{\underbrace{\underset{x}{\min }L(x,\mu )}},L(x,\mu )=\frac{1}{2}{x}^{\top }Qx+c^{\top }x+{\mu }^{\top }(Ax-b)\\ \text{s.t.}& \mu \ge 0\\ & \mu \in \{\mu \mid q(\mu )>-\infty \},\end{array}$$ where the Lagrangian can be manipulated into the form $$L(x,\mu )=\frac{1}{2}{x}^{\top }Qx+x^{\top }(c+A^{\top }\mu )-b^{\top }\mu .$$ Its critical point can be found by solving $$\frac{\partial L}{\partial x}(x,\mu )=Qx+c+A^{\top }\mu =0,$$ which yields $$x=-Q^{-1}(c+A^{\top }\mu ).$$ When substituted back into the original dual problem, after basic manipulations, we attain $$\begin{array}{rl}\underset{\mu }{\max }& q(\mu )=-\frac{1}{2}(c+A^{\top }\mu {)}^{\top }{Q}^{-1}(c+A^{\top }\mu )-b^{\top }\mu \\ \text{s.t.}& \mu \ge 0\\ & \mu \in \{\mu \mid q(\mu )>-\infty \}.\end{array}$$

Bellman Principle

According to Bellman1966 “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.” When applied recursively, this means that decisions at all states of an optimal trajectory must appear to be optimal when considering only the future behavior of the system. While the statement might sound rather vague, Bellman’s principle is the foundation of optimal control, applicable to both continuous-time and discrete-time systems and trajectories analyzed not only on finite but also infinite time horizons.

In the following subsections we will overview the Bellman and the Hamilton-Jacobi-Bellman (HJB) equation which are the consequence of Bellman’s principle when applied to the discrete-time and continuous-time optimal control problems. Regardless of the representation, the idea is find a sequence of control inputs that minimize the total cost of a trajectory, defined on a horizon. Instead of looking for the entire sequence at once, the Bellman principle allows us to break it down into a sequence of problems, described by the aforementioned equations. For their derivation we introduce the concept of the cost-to-go, i.e. the remainder of the total cost from any point on the horizon to its end, assuming a particular sequence of inputs. In the case of the optimal sequence of inputs we refer to the corresponding cost-to-go as the value function. The value function figures on the left-hand side of both equations where is expressed recursively.

This is high level overview which without additional context might be incomprehensible. Hopefully the following subsections will provide the necessary context in the form of rigorous derivations and later also applications. Especially the derivations are likely to be sparse on text, so always feel free to jump back.

To emphasize the relevance of Bellman’s principle in present day research a mention of the differential dynamic programming algorithm is called for. While the original algorithm was introduces in Mayne1966 its extensions are vibrant area of contemporary research.

Bellman Equation

For a discrete-time linear system, with dynamics in the form $$x_{k+1}=f(x_k,u_k,k),$$ let us assume we are trying to minimize the total cost $$J(x_0,u_{0:N-1},0)=\Phi (x_N)+\sum _{i=0}^{N-1}l(x_i,u_i,i).$$ of a trajectory $(x_{0:N},u_{0:N-1})$ where the initial state $x_0$ is prescribed. The concept of the total cost can be generalized for any $k\in [0,N]$ to a cost-to-go $$J(x_k,u_{k:N-1},k)=\Phi (x_N)+\sum _{i=k}^{N-1}l(x_i,u_i,i),$$ for which we may define a value function $$\begin{array}{rl}V(x_k,k)=\underset{u_{k:N-1}}{\min }& J(x_k,u_{k:N-1},k)\\ \text{s.t.}& x_{i+1}=f(x_i,u_i,i)\forall i\in [k,N-1]\end{array}$$ and an optimal control policy $$\begin{array}{rl}{u}_{k:N-1}^{\ast }=\underset{u_{k:N-1}}{\mathrm{argmin}}& J(x_k,u_{k:N-1},k)\\ \text{s.t.}& x_{i+1}=f(x_i,u_i,i)\forall i\in [k,N-1],\end{array}$$ the application of which results in the system following the optimal sequence of states $x_{k+1:N}^{\ast }$.

The so-called Bellman equation can then be derived by formulating the value function for step $k$ recursively, using the value function for step $k+1$. From its definition, it is apparent that $$V(x_k,k)=l(x_k,u_k^{\ast },k)+V(x_{k+1}^{\ast },k+1),x_{k+1}^{\ast }=f(x_k,u_k^{\ast },k),\text{\hspace{1em}(1)}$$ where $$V(x_N,N)=\Phi (x_N).$$ Assuming we don’t know what the optimal input at step $k$ is, we may pose the right-hand side of (1) as an optimization problem $$V(x_k,k)=\underset{u_k}{\min }\left(l(x_k,u_k,k)+V(f(x_k,u_k,k),k+1)\right).\text{\hspace{1em}(2)}$$ Equation (2) is then referred to as the Bellman equation.

Hamilton-Jacobi-Bellman Equation

Let us assume we are trying to minimize the total cost $$J(x(t_0),u(\tau ),t_0)=\Phi (x(t_f))+{\int }_{t_0}^{t_f}l(x(\tau ),u(\tau ),\tau )d\tau .$$ of a continuous-time system’s trajectory $x(\tau )$, $\tau \in [t_0,t_f]$ with dynamics in the form $$\dot{x}(t)=f(x(t),u(t),t),$$ starting from the state $x(t_0)={\tilde{x}}_0$.

The concept of the total cost can be generalized for any $t\in [t_0,t_f]$ to a cost-to-go $$J(x(t),u(\tau ),t)=\Phi (x(t_f))+{\int }_t^{t_f}l(x(\tau ),u(\tau ),\tau )d\tau ,$$ for which we may define a value function $$\begin{array}{rl}V(x(t),t)=\underset{u(\tau )}{\min }& J(x(t),u(\tau ),t),\tau \in [t,t_f),\\ \text{s.t.}& \dot{x}(\tau )=f(x(\tau ),u(\tau ),\tau ),\forall \tau \in [t,t_f]\end{array}$$ and optimal control policy $$\begin{array}{rl}{u}^{\ast }(\tau )=\underset{u(\tau )}{\mathrm{argmin}}& J(x(t),u(\tau ),t),\tau \in [t,t_f),\\ \text{s.t.}& \dot{x}(\tau )=f(x(\tau ),u(\tau ),\tau ),\forall \tau \in [t,t_f]\end{array}$$ the application of which results in the system following an optimal trajectory $x^{\ast }(\tau )$, $\tau \in (t,t_f]$.

For the previously defined value function the Hamilton-Jacobi-Bellman (HJB) equation can be derived¹ as $$-\frac{\partial V}{\partial t}(x(t),t)=\underset{u(t)}{\min }\left(l(x(t),u(t),t)+{\left(\frac{\partial V}{\partial x}\right)}^{\top }(x(t),t)f(x(t),u(t),t)\right).$$

1. An overview of the derivation is presented by Steven Brunton in one of his videos Brunton2022-HJB also available here. As a note, there is a small mistake, acknowledged by the presenter in the comments, at 9:11 of the video where “$0$” should be replaced with “$t$”. ↩

Linear-Quadratic Regulator

The linear-quadratic regulator gets its name from the linear model of the system and the quadratic cost function used for its design. For these, the optimal controller can be derived analytically and takes the form of linear state feedback making its implementation particularly simple.

The systems can be described in both the discrete-time and continuous-time domain and particular variants of the LQR allow for time-variant systems. Cost functions must then reflect the type of the system. For time-invariant systems, the cost function can either take the form of a time-invariant running cost integrated over an infinite horizon, or a sum of a final cost and a running cost (optionally time-variant) integrated over a finite horizon. If the system is time-variant, only the second option is available.

In the following sections we will cover four variants based on the time-domain and horizon

• discrete-time x continuous-time,
• finite-horizon x infinite-horizon.

Many more variants exist including those for reference tracking, dead-beat control, and even nonlinear trajectory optimization.

Discrete-Time Finite-horizon Linear-Quadratic Regulator

For a linear time-variant discrete-time system $$x_{k+1}=\underset{f(x_k,u_k,k)}{\underbrace{A_k{x}_k+B_k{u}_k}}$$ and a quadratic total cost $$J(x_0,u_{0:N-1},0)=\underset{\Phi (x_N)}{\underbrace{x_N^{\top }{Q}_N{x}_N}}+\sum _{i=0}^{N-1}\underset{l(x_i,u_i,i)}{\underbrace{x_i^{\top }{Q}_i{x}_i+u_i^{\top }{R}_i{u}_i}},Q_i⪰0,R_i\succ 0$$ of its trajectory $(x_{0:N},u_{0:N-1})$ the optimal controller can be derived based on the assumption that the value function takes the form $$V(x_k,k)=x_k^{\top }{P}_k{x}_k,P_k⪰0.$$ When substituted into the Bellman Equation along with the system’s dynamics we attain $$x_k^{\top }{P}_k{x}_k=\underset{u_k}{\min }\left\{x_k^{\top }{Q}_k{x}_k+u_k^{\top }{R}_k{u}_k+(A_k{x}_k+B_k{u}_k{)}^{\top }{P}_{k+1}(A_k{x}_k+B_k{u}_k)\right\}.\text{\hspace{1em}(1)}$$ To find the minimum, we may take the gradient of its argument (which is by design quadratic and convex) with respect to $u_k$, set it to zero and find the solution (optimal control input) $$u_k^{\ast }=-{\left(R_k+B_k^{\top }{P}_{k+1}{B}_k\right)}^{-1}{B}_k^{\top }{P}_{k+1}{A}_k{x}_k.$$

The input can then be substituted back into (1). As the equation must hold for all $x_k$, through basic manipulations we then attain the discrete-time dynamic Riccati equation (DDRE)

$$P_k=Q_k+A_k^{\top }{P}_{k+1}{A}_k-A_k^{\top }{P}_{k+1}{B}_k{\left(B_k^{\top }{P}_{k+1}{B}_k+R_k\right)}^{-1}{B}_k^{\top }{P}_{k+1}{A}_k,P_N=Q_N$$ for a finite horizon $N\in \mathbb{N}$.

Discrete-Time Infinite-horizon Linear-Quadratic Regulator

For a linear time-invariant discrete-time system $$x_{k+1}=\underset{f(x_k,u_k,k)}{\underbrace{A{x}_k+B{u}_k}}$$ and a quadratic total cost $$J(x_0,u_{0:\infty },0)=\sum _{i=0}^{\infty }\underset{l(x_i,u_i,i)}{\underbrace{x_i^{\top }Q{x}_i+u_i^{\top }R{u}_i}},Q⪰0,R\succ 0$$ of its trajectory $(x_{0:\infty },u_{0:\infty })$ the optimal controller can be derived based on the assumption that the value function on the infinite horizon takes the time-invariant form $$V(x_k,k)=x_k^{\top }P{x}_k,P\succ 0.$$

When substituted into the Bellman Equation along with the system’s dynamics we attain $$x_k^{\top }P{x}_k=\underset{u_k}{\min }\left\{x_k^{\top }Q{x}_k+u_k^{\top }R{u}_k+(A{x}_k+B{u}_k{)}^{\top }P(A{x}_k+B{u}_k)\right\}.\text{\hspace{1em}(1)}$$ To find the minimum, we may take the gradient of its argument (which is by design quadratic and convex) with respect to $u_k$, set it to zero and find the solution (optimal control input) $$u_k^{\ast }=-{\left(R+B^{\top }PB\right)}^{-1}{B}^{\top }PA{x}_k.$$

The input can then be substituted back into (1). As the equation must hold for all $x_k$, through basic manipulations we then attain the discrete-time algebraic Riccati equation (DARE) $$P=Q+A^{\top }PA-A^{\top }PB{\left(B^{\top }PB+R\right)}^{-1}{B}^{\top }PA,P\succ 0.$$