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 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.

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{\mathbf{x}}{\max }& {\mathbf{c}}^{\top }\mathbf{x}\\ \text{s.t.}& \mathbf{A}\mathbf{x}\le \mathbf{b}\\ & \mathbf{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}\mathbf{c}+{\mathbf{A}}^{\top }{\mathbf{\mu }}_1-{\mathbf{\mu }}_2& =0& \text{stationarity}\\ \mathbf{A}\mathbf{x}& \le \mathbf{b}& \text{primal feasibility}\\ \mathbf{x}& \ge 0& \\ {\mathbf{\mu }}_1& \ge 0& \text{dual feasibility}\\ {\mathbf{\mu }}_2& \ge 0& \\ {\mathbf{\mu }}_1^{\top }(\mathbf{A}\mathbf{x}-\mathbf{b})& =0& \text{complementary slackness}\\ -{\mathbf{\mu }}_2^{\top }\mathbf{x}& =0& \end{array}$$

Dual problem

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

Quadratic Programming

Let us consider a constrained optimization problem in the form $$\begin{array}{rl}\underset{\mathbf{x}}{\min }& \frac{1}{2}{\mathbf{x}}^{\top }\mathbf{Q}\mathbf{x}+{\mathbf{c}}^{\top }\mathbf{x}\\ \text{s.t.}& \mathbf{A}\mathbf{x}\le \mathbf{b},\end{array}$$ where $\mathbf{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}\mathbf{Q}\mathbf{x}+\mathbf{c}+{\mathbf{A}}^{\top }\mathbf{\mu }& =0& \text{stationarity}\\ \mathbf{A}\mathbf{x}& \le \mathbf{b}& \text{primal feasibility}\\ \mathbf{\mu }& \ge 0& \text{dual feasibility}\\ {\mathbf{\mu }}^{\top }(\mathbf{A}\mathbf{x}-\mathbf{b})& =0& \text{complementary slackness}\end{array}$$

Dual problem

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

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 $$V(x_k,k)=\underset{u_{k:N-1}}{\min }J(x_k,u_{k:N-1},k),$$ and an optimal control policy $$u_{k:N-1}^{\ast }=\underset{u_{k:N-1}}{\mathrm{argmin}}J(x_k,u_{k:N-1},k),$$ 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(\mathbf{x}(t_0),\mathbf{u}(\tau ),t_0)=\Phi (\mathbf{x}(t_f))+{\int }_{t_0}^{t_f}l(\mathbf{x}(\tau ),\mathbf{u}(\tau ),\tau )d\tau .$$ of a continuous-time system's trajectory $\mathbf{x}(\tau )$, $\tau \in ⟨t_0,t_f⟩$ with dynamics in the form $$\dot{\mathbf{x}}(t)=\mathbf{f}(\mathbf{x}(t),\mathbf{u}(t),t),$$ starting from the state $\mathbf{x}(t_0)={\tilde{\mathbf{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(\mathbf{x}(t),\mathbf{u}(\tau ),t)=\Phi (\mathbf{x}(t_f))+{\int }_t^{t_f}l(\mathbf{x}(\tau ),\mathbf{u}(\tau ),\tau )d\tau ,$$ for which we may define a value function $$V(\mathbf{x}(t),t)=\underset{\mathbf{u}(\tau )}{\min }J(\mathbf{x}(t),\mathbf{u}(\tau ),t),$$ and optimal control policy $${\mathbf{u}}^{\ast }(\tau )=\underset{\mathbf{u}(\tau )}{\mathrm{argmin}}J(\mathbf{x}(t),\mathbf{u}(\tau ),t),\tau \in ⟨t,t_f)$$ the application of which results in the system following an optimal trajectory ${\mathbf{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}(\mathbf{x}(t),t)=\underset{\mathbf{u}(t)}{\min }\left(l(\mathbf{x}(t),\mathbf{u}(t),t)+{\left(\frac{\partial V}{\partial \mathbf{x}}\right)}^{\top }(\mathbf{x}(t),t)\mathbf{f}(\mathbf{x}(t),\mathbf{u}(t),t)\right).$$

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 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 }S{x}_k,S\succ 0.$$

When substituted into the Bellman Equation along with the system's dynamics we attain $$x_k^{\top }S{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 }S(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 }SB\right)}^{-1}{B}^{\top }SA{x}_k.$$

The input can then be substitued 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) $$S=Q+A^{\top }SA-A^{\top }SB{\left(B^{\top }SB+R\right)}^{-1}{B}^{\top }SA,S\succ 0.$$

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{x}_i+u_i^{\top }R{u}_i}},Q_N⪰0,Q⪰0,R\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 }{S}_k{x}_k,S_k\succ 0.$$ When substituted into the Bellman Equation along with the system's dynamics we attain $$x_k^{\top }{S}_k{x}_k=\underset{u_k}{\min }\left\{x_k^{\top }Q{x}_k+u_k^{\top }R{u}_k+(A_k{x}_k+B_k{u}_k{)}^{\top }{S}_{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+B_k^{\top }{S}_{k+1}{B}_k\right)}^{-1}{B}_k^{\top }{S}_{k+1}{A}_k{x}_k.$$

The input can then be substitued 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) $$S_k=Q+A_k^{\top }{S}_{k+1}{A}_k-A_k^{\top }{S}_{k+1}{B}_k{\left(B_k^{\top }{S}_{k+1}{B}_k+R\right)}^{-1}{B}_k^{\top }{S}_{k+1}{A}_k,S_N=Q_N$$ for a finite horizon $N\in \mathbb{N}$.

Continuous-Time Infinite-Horizon Linear-Quadratic Regulator

For a linear time-invariant continuous-time system $$\dot{x}(t)=\underset{f(x(t),u(t),t)}{\underbrace{Ax(t)+Bu(t)}}$$ and a quadratic total cost $$J(x(t_0),u(\tau ),t_0)={\int }_{t_0}^{\infty }\underset{l(x(\tau ),u(\tau ),\tau )}{\underbrace{x(\tau {)}^{\top }Qx(\tau )+u(\tau {)}^{\top }Ru(\tau )}}d\tau ,$$ where $Q⪰0$ and $R\succ 0$, of its trajectory $(x(\tau ),u(\tau ))$, $\tau \in (t_0,\infty )$ the optimal controller can be derived based on the assumption that the value function takes the form $$V(x(t),t)=x(t{)}^{\top }Sx(t),S\succ 0.$$ When substituted into the Hamilton-Jacobi-Bellman Equation along with the system's dynamics we attain $$0=\underset{u(t)}{\min }\left\{x(t{)}^{\top }Qx(t)+u(t{)}^{\top }Ru(t)+2x(t{)}^{\top }S\left(Ax(t)+Bu(t)\right)\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(t)$, set it to zero and find the solution (optimal control input) $$u^{\ast }(t)=-R^{-1}{B}^{\top }Sx(t).$$

The input can then be substituted back into (1). As the equation must hold for all $x(t)$, through basic manipulations we then attain the continuous-time algebraic Riccati equation (CARE) $$0=Q-S{B}^{\top }{R}^{-1}BS+SA+A^{\top }S,S\succ 0.$$

Continuous-Time Infinite-Horizon Linear-Quadratic Regulator

For a linear time-variant continuous-time system $$\dot{x}(t)=\underset{f(x(t),u(t),t)}{\underbrace{A(t)x(t)+B(t)u(t)}}$$ and a quadratic total cost $$J(x(t_0),u(\tau ),t_0)=\underset{\Phi (x_N)}{\underbrace{x(t_f{)}^{\top }{Q}_{f}x(t_f)}}+{\int }_{t_0}^{t_f}\underset{l(x(\tau ),u(\tau ),\tau )}{\underbrace{x(\tau {)}^{\top }Qx(\tau )+u(\tau {)}^{\top }Ru(\tau )}}d\tau ,$$ where $Q_f⪰0$, $Q⪰0$, and $R\succ 0$, of its trajectory $(x(\tau ),u(\tau ))$, $\tau \in ⟨t_0,t_f⟩$ the optimal controller can be derived based on the assumption that the value function takes the form $$V(x(t),t)=x(t{)}^{\top }S(t)x(t),S(t)\succ 0.$$ When substituted into the Hamilton-Jacobi-Bellman Equation along with the system's dynamics we attain $$\begin{array}{c}-x(t{)}^{\top }\dot{S}(t)x(t)=\\ \underset{u(t)}{\min }\left\{x(t{)}^{\top }Qx(t)+u(t{)}^{\top }Ru(t)+2x(t{)}^{\top }S(t)\left(A(t)x(t)+B(t)u(t)\right)\right\}.\end{array}\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(t)$, set it to zero and find the solution (optimal control input) $$u^{\ast }(t)=-R^{-1}B(t{)}^{\top }S(t)x(t).$$

The input can then be substituted back into (1). As the equation must hold for all $x(t)$, through basic manipulations we then attain the continuous-time differential Riccati equation (CDRE) $$-\dot{S}(t)=Q-S(t)B(t{)}^{\top }{R}^{-1}B(t)S(t)+S(t)A(t)+A(t{)}^{\top }S(t)$$ for a finite horizon $t_f\in \mathbb{R}$. Supplemented with the boundary conditon $S(t_f)=Q_f$, the CDRE forms a initial value problem (IVP) which can be solved using numerical integration. Numerical errors in the integration process may lead to the loss of positive-semi-definiteness. To overcome this, instead of integrating $S(t)$ directly, we may use its factorized form $S(t)=P(t)P^{\top }(t)$ a.k.a. the "square-root form" where $$-\dot{P}(t)=\frac{1}{2}Q{P}^{-\top }(t)-\frac{1}{2}S(t)B(t)R^{-1}B(t{)}^{\top }P(t)+A(t{)}^{\top }P(t).$$ As $P(t)$ must be invertible $Q_f$ must be (at least numerically) positive definite. Consequenlty we may use the cholesky factorization in order to form the boundary condition $$P(t_f)=L,Q_f=L{L}^{\top }.$$

Linear-Quadratic MPC

Model predictive control (MPC) is a control strategy which relies on the system's model to predict and optimize its trajectory online, periodically re-optimizing the trajectory as a form of feedback. If a linear representation of the system and a quadratic running and final cost are used, it is essentially an extension of the finite-horizon LQR that includes control in state inputs. This is in fact what is commonly referred to as MPC. If the system's dynamics are nonlinear or the cost functions contain terms that are not linear nor quadratic, the strategy is referred to as nonlinear MPC.

In this course we will limit ourselves only to the former case which can be transcribed into a QP problem. We will go through two formulations of this problem based on how the system's dynamics are incorporated into the optimization problem:

• explicitly constrained - both states ${\mathbf{x}}_k$ and inputs ${\mathbf{u}}_k$ are the decision variables,
• implicitly constrained - only inputs ${\mathbf{u}}_k$ are decision variables.

Direct Linear-Quadratic MPC

The standard realization of an MPC controller is such that solves the optimization problem $$\begin{array}{rl}\underset{\{{\mathbf{x}}_k{\}}_{k=1}^N,\{{\mathbf{u}}_k{\}}_{k=0}^{N-1}}{\min }& {\mathbf{x}}_N^{\top }{\mathbf{Q}}_N{\mathbf{x}}_N+\sum _{k=0}^{N-1}{\mathbf{x}}_k^{\top }\mathbf{Q}{\mathbf{x}}_k+{\mathbf{u}}_k^{\top }\mathbf{R}{\mathbf{u}}_k\\ \text{s.t.}& {\mathbf{x}}_{k+1}=\mathbf{A}{\mathbf{x}}_k+\mathbf{B}{\mathbf{u}}_k,k=0,\dots ,N-1\\ & {\mathbf{u}}_{\min }\le {\mathbf{u}}_k\le {\mathbf{u}}_{\max },k=0,\dots ,N-1\end{array}$$ where the initial state ${\mathbf{x}}_0$ is given¹.

Canonical QP form

Optimized variables

$$\begin{array}{c}\mathbf{x}={\left[\begin{array}{cccccccc}{\mathbf{u}}_0^{\top }& \dots & \dots & {\mathbf{u}}_{N-1}^{\top }& {\mathbf{x}}_1^{\top }& \dots & \dots & {\mathbf{x}}_N^{\top }\end{array}\right]}^{\top }\end{array}$$

Objective function's terms

$$\begin{array}{c}\mathbf{P}=\left[\begin{array}{cccccccc}\mathbf{R}& & & & & & & \\ & \ddots & & & & & & \\ & & \ddots & & & & & \\ & & & \mathbf{R}& & & & \\ & & & & \mathbf{Q}& & & \\ & & & & & \ddots & & \\ & & & & & & \ddots & \\ & & & & & & & \mathbf{Q}\end{array}\right],\mathbf{q}=\left[\begin{array}{c}0\\ ⋮\\ ⋮\\ 0\\ 0\\ ⋮\\ ⋮\\ 0\end{array}\right]\end{array}$$

Constraint's terms

$$\begin{array}{c}\mathbf{l}=\left[\begin{array}{c}-\mathbf{A}{\mathbf{x}}_0\\ 0\\ ⋮\\ 0\\ {\mathbf{u}}_{\min }\\ ⋮\\ ⋮\\ {\mathbf{u}}_{\min }\end{array}\right],\mathbf{A}=\left[\begin{array}{cccccccc}\mathbf{B}& & & & -\mathbf{I}& & & \\ & \mathbf{B}& & & \mathbf{A}& -\mathbf{I}& & \\ & & \ddots & & & \ddots & \ddots & \\ & & & \mathbf{B}& & & \mathbf{A}& -\mathbf{I}\\ \mathbf{I}& & & & & & & \\ & \ddots & & & & & & \\ & & \ddots & & & & & \\ & & & \mathbf{I}& & & & \end{array}\right],\mathbf{u}=\left[\begin{array}{c}-\mathbf{A}{\mathbf{x}}_0\\ 0\\ ⋮\\ 0\\ {\mathbf{u}}_{\max }\\ ⋮\\ ⋮\\ {\mathbf{u}}_{\max }\end{array}\right]\end{array}$$

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}I+a_{1}A+\dots +a_{n-1}{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}{u}_0)+A^{n-2}B(u_2+a_{n-2}{u}_0)+\dots +B(u_n+a_0{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{u}_0\\ u_{n-1}+a_1{u}_0\\ u_1+a_{n-1}{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.

Discretization of LTI Systems

Let us consider a linear time-invariant system in the form $$\dot{x}(\tau )=Ax(\tau )+Bu(\tau ).$$

Integration of the system's dynamics over an iterval $\tau \in ⟨0,t+h⟩$ gives us $$x(t+h)=e^{A(t+h)}x(0)+{\int }_0^{t+h}{e}^{A(t+h-\tau )}Bu(\tau )d\tau .$$ To express its state at $x(t+h)$ with respect to the state $x(t)$ we may perform a series of manipulations $$\begin{array}{rl}x(t+h)& =e^{A(t+h)}x(0)+{\int }_0^{t+h}{e}^{A(t+h-\tau )}Bu(\tau )d\tau \\ & =e^{A(t+h)}x(0)+{\int }_0^t{e}^{A(t+h-\tau )}Bu(\tau )d\tau +{\int }_t^{t+h}{e}^{A(t+h-\tau )}Bu(\tau )d\tau \\ & =e^{Ah}\underset{x(t)}{\underbrace{\left(e^{At}x(0)+{\int }_0^t{e}^{A(t-\tau )}Bu(\tau )d\tau \right)}}+{\int }_t^{t+h}{e}^{A(t+h-\tau )}Bu(\tau )d\tau .\end{array}\text{\hspace{1em}(1)}$$ If we then set $u(\tau )=\hat{u}$, $\tau \in ⟨t,t+h)$ the integral $${\int }_t^{t+h}{e}^{A(t+h-\tau )}Bu(\tau )d\tau$$ can be manipulated into $$\begin{array}{rl}{\int }_t^{t+h}{e}^{A(t+h-\tau )}Bu(\tau )d\tau & =e^{A(t+h)}{\int }_t^{t+h}{e}^{-A\tau }d\tau B\hat{u}\\ & =e^{A(t+h)}{\left[-A^{-1}{e}^{-A\tau }\right]}_t^{t+h}B\hat{u}\\ & =e^{A(t+h)}\left(-A^{-1}{e}^{-A(t+h)}+A^{-1}{e}^{-At}\right)B\hat{u}\\ & =A^{-1}\left(e^{Ah}-I\right)B\hat{u}.\end{array}\text{\hspace{1em}(2)}$$ By substituting (2) into (1) we obtain $$x_{k+1}=e^{Ah}{x}_k+A^{-1}(e^{Ah}-I)B{u}_k\text{\hspace{1em}(3)}$$ where $x_{k+1}=x(t+h)$, $x_k=x(t)$, and $u_k=\hat{u}$.

If we use the first two terms of the taylor expansion $$e^{Ah}=I+Ah+\frac{(Ah{)}^2}{2!}+\dots$$ in (3) we get the system representation $$x_{k+1}=(I+Ah)x_k+Bh{u}_k.$$

Optimal Luenberger Observer

Finite horizon

Let us consider a LTI system with additive Gaussian noise $$\begin{array}{rlrl}{x}_k& =A{x}_{k-1}+B{u}_{k-1}+w_{k-1},& Q_{k-1}& =\mathbb{E}[w_k{w}_k^{\top }]\\ y_k& =C{x}_k+v_k,& R_{k-1}& =\mathbb{E}[v_k{v}_k^{\top }].\end{array}$$ and a Luenberger observer $${\hat{x}}_k=A{\hat{x}}_{k-1}+B{u}_{k-1}-L_k(C{\hat{x}}_{k-1}-y_{k-1}).\text{\hspace{1em}(1)}$$ The state estimate's error $e_k={\hat{x}}_k-x_k$ and its covariance $P_k=\mathbb{E}[e_k{e}_k^{\top }]$ can be expressed recursively as $$\begin{array}{rl}{e}_k& =w_{k-1}+(A-L_{k}C)e_{k-1}+L_k{v}_{k-1}\\ P_k& =Q_{k-1}+(A-L_{k}C)P_{k-1}(A-L_{k}C{)}^{\top }+L_k{R}_{k-1}{L}_k^{\top }.\end{array}\text{\hspace{1em}(2)}$$ The gain $L_k$ can then be designed such that $P_k$ is minimized. This can be achieved by finding the stationary point $\frac{\partial P_k}{\partial L_k}=0$, where $$L_k=P_{k-1}{C}^{\top }(C{P}_{k-1}{C}^{\top }+R_{k-1}{)}^{-1}.\text{\hspace{1em}(3)}$$ Substituting (3) back into (2b) we attain the DDRE $$P_k=Q_{k-1}+A{P}_{k-1}{A}^{\top }-A{P}_{k-1}{C}^{\top }(C{P}_{k-1}{C}^{\top }+R_{k-1}{)}^{-1}C{P}_{k-1}{A}^{\top },\text{\hspace{1em}(4)}$$ which governs the evolution of the state error's covariance.

The state of the observer then consists of ${\hat{x}}_k$ and $P_k$, its dynamics described by (1) and (4). This observer can also be used in the case where the dynamics of the system are time variant.

Infinite horizon

In the case where the covariances of the process and measurement noise are time-invariant, i.e. $$Q=\mathbb{E}[w_k{w}_k^{\top }],R=\mathbb{E}[v_k{v}_k^{\top }],$$ we can also design the gain of the observer based on the steady-state error covariance $P_k\to P$ at $k\to \infty$. The covariance $P$ can be obtained by solving the DARE $$P=Q+AP{A}^{\top }-AP{C}^{\top }(CP{C}^{\top }+R{)}^{-1}CP{A}^{\top },\text{\hspace{1em}(5)}$$ based on which we evaluate the gain $$L=P{C}^{\top }(CP{C}^{\top }+R{)}^{-1}.$$

Duality with LQR

It is of note that the form of (4) and (5) is identical to those of the LQR with different matrices:

Kalman Filter

Let us consider a LTI system with additive Gaussian noise $$\begin{array}{rlrl}{x}_k& =A{x}_{k-1}+B{u}_{k-1}+w_{k-1},& Q_k& =\mathbb{E}[w_k{w}_k^{\top }]\\ y_k& =C{x}_k+v_k,& R_k& =\mathbb{E}[v_k{v}_k^{\top }].\end{array}$$ In addition to the state estimate ${\hat{x}}_k$ the state of the Kalman Filter includes the error covariance $$P_k=\mathbb{E}[e_k{e}_k^{\top }],$$ where $e_k={\hat{x}}_k-x_k$ is the state estimate's error.