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

The materials are loosely divided into three blocks. The first focuses on the classical approaches in optimal control, namely the four basic variants of the linear quadratic regulator (LQR) which are derived from the Bellman principle. The second goes over the basics of numerical optimization building up-to the third and final block focusing on model predictive control (MPC).

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

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 being applied to the discrete-time and continuous-time optimal control problems. Later we will use both the Bellman and HJB equation to derive variants of the linear-quadratic regulator (LQR), a staple in optimal control.

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 the product of research in the current decade.

Bellman Equation

Let us assume we are trying to minimize the total cost $$J({\mathbf{x}}_0,\{{\mathbf{u}}_i{\}}_{i=0}^{N-1},0)=\Phi ({\mathbf{x}}_N)+\sum _{i=0}^{N-1}l({\mathbf{x}}_i,{\mathbf{u}}_i,i).$$ of a discrete-time system's trajectory $\{{\mathbf{x}}_i{\}}_{i=0}^N$ with dynamics in the form $$x_{k+1}=\mathbf{f}({\mathbf{x}}_k,{\mathbf{u}}_k,k),$$ starting from the state ${\mathbf{x}}_0={\tilde{\mathbf{x}}}_0$.

The concept of the total cost can be generalized for any $k\in ⟨0,N⟩$ to a cost-to-go $$J({\mathbf{x}}_k,\{{\mathbf{u}}_i{\}}_{i=k}^{N-1},k)=\Phi ({\mathbf{x}}_N)+\sum _{i=k}^{N-1}l({\mathbf{x}}_i,{\mathbf{u}}_i,i),$$ for which we may define a value function $$V({\mathbf{x}}_k,k)=\underset{\{{\mathbf{u}}_i{\}}_{i=k}^{N-1}}{\min }J({\mathbf{x}}_k,\{{\mathbf{u}}_i{\}}_{i=k}^{N-1},k),$$ and an optimal control policy $$\{{\mathbf{u}}_i^{\ast }{\}}_{i=k}^{N-1}=\underset{\{{\mathbf{u}}_i{\}}_{i=k}^{N-1}}{\mathrm{argmin}}J({\mathbf{x}}_k,\{{\mathbf{u}}_i{\}}_{i=k}^{N-1},k),$$ the application of which results in the system following an optimal trajectory $\{{\mathbf{x}}_i^{\ast }{\}}_{i=k}^N$.

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$) as $$V({\mathbf{x}}_k,k)=\underset{{\mathbf{u}}_k}{\min }\left(l({\mathbf{x}}_k,{\mathbf{u}}_k,k)+V({\mathbf{x}}_{k+1},k+1)\right)$$ and substituting ${\mathbf{x}}_{k+1}$ using the system's dynamics to attain the final form $$V({\mathbf{x}}_k,k)=\underset{{\mathbf{u}}_k}{\min }\left(l({\mathbf{x}}_k,{\mathbf{u}}_k,k)+V(\mathbf{f}({\mathbf{x}}_k,{\mathbf{u}}_k,k),k+1)\right).$$

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

Discrete-Time Linear-Quadratic Regulator

Infinite horizon

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_i{\}}_{i=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_i{\}}_{i=0}^{\infty }$ 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{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.$$

Finite horizon

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_i{\}}_{i=0}^N,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_i{\}}_{i=0}^N$ 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{x}_k+B{u}_k{)}^{\top }{S}_{k+1}(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 }{S}_{k+1}B\right)}^{-1}{B}^{\top }{S}_{k+1}A{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^{\top }{S}_{k+1}A-A^{\top }{S}_{k+1}B{\left(B^{\top }{S}_{k+1}B+R\right)}^{-1}{B}^{\top }{S}_{k+1}A,S_N=Q_N$$ for a finite horizon $N\in \mathbb{N}$.

Continuous-Time Linear-Quadratic Regulator

Infinite horizon

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 )$, $\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 }Sx(t),S\succ 0.$$ When substituted into the Hamilton-Jacobi-Bellman Equation along with the system's dynamics we attain $$0=\underset{u}{\min }\left\{x^{\top }Qx+u^{\top }Ru+2x^{\top }S\left(Ax+Bu\right)\right\},\text{\hspace{1em}(1)}$$ where $x=x(t)$ and $u=u(t)$. 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.$$

Finite horizon

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)=\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 )$, $\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 $$-x^{\top }\dot{S}x=\underset{u}{\min }\left\{x^{\top }Qx+u^{\top }Ru+2x^{\top }S\left(Ax+Bu\right)\right\},\text{\hspace{1em}(1)}$$ where $x=x(t)$ and $u=u(t)$. 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 }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^{\top }{R}^{-1}BS(t)+S(t)A+A^{\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{R}^{-1}{B}^{\top }P(t)+A^{\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 }.$$

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}$$

LP Problem Examples

Car importer

A car importer is planning to import two models of JDM cars. In one shipment he can transport 60 cars and investment of 50 million CZK. The purchase price of the two models is 2.5 million and 0.5 million CZK. He then expects a profit of 250 thousand CZK per unit sold of the first model and 75 thousand CZK of the second model. How many units of each model should he buy to maximize his profit, expecting every car will be sold?

Mathematical model

$$\begin{array}{rl}\underset{x_1,x_2}{\max }& 0.25x_1+0.075x_2\\ \text{s.t.}& x_1\ge 0,x_2\ge 0\\ & x_1+x_2\le 60\\ & 2.5x_1+0.5x_2\le 50\end{array}$$

Code

• julia - JuMP
• python - CVXPY

Flow optimization

We have a network (represented as directed graph). The graph has a "Source" of a medium (it can be for example gas, water or electricity) and Sink with additional nodes in between. All edges between source and sink has defined maximal capacity.

graph LR;
    A["Source"];
		D["Sink"];
		A-->|"10"|B1;
    A-->|"8"|B2;
    A-->|"5"|C1;
    B1-->|"6"|C1;
		B1-->|"2"|C2;
		B2-->|"4"|C1;
		B2-->|"5"|C2;
		C1-->|"8"|D;
		C2-->|"9"|D;

What is the maximum amount medium we can transmit through the sink?

Mathematical model

Let us first define our optimization variables $x_i$ as individual flows through the edges of our graph

graph LR;
    A["Source"];
		D["Sink"];
		A-->|"x₂ ≤ 10"|B1;
    A-->|"x₃ ≤ 8"|B2;
    A-->|"x₁ ≤ 5"|C1;
    B1-->|"x₄ ≤ 6"|C1;
		B1-->|"x₅ ≤ 2"|C2;
		B2-->|"x₆ ≤ 4"|C1;
		B2-->|"x₇ ≤ 5"|C2;
		C1-->|"x₈ ≤ 8"|D;
		C2-->|"x₉ ≤ 9"|D;

we may then write the problem as

$$\begin{array}{rl}\underset{\mathbf{x}}{\max }& x_8+x_9\\ \text{s.t.}& 0\le \mathbf{x}\le \mathbf{u}\\ & x_4+x_5=x_2\\ & x_6+x_7=x_3\\ & x_8=x_1+x_6+x_4\\ & x_9=x_5+x_7\end{array}$$ where $${\mathbf{u}}^{\top }=\left[\begin{array}{ccccccccc}5& 10& 8& 6& 2& 4& 5& 8& 9\end{array}\right]$$

Canonical form

Manipulating the objective into the canonical form can be done simply with $${\mathbf{c}}^{\top }=\left[\begin{array}{ccccccccc}0& 0& 0& 0& 0& 0& 0& 1& 1\end{array}\right].$$ To deal with the constraints let us first formulate the equality constraints in matrix form as: $${\mathbf{A}}_{\mathrm{eq}}\mathbf{x}=0$$ where $${\mathbf{A}}_{\mathrm{eq}}=\left[\begin{array}{ccccccccc}0& -1& 0& 1& 1& 0& 0& 0& 0\\ 0& 0& -1& 0& 0& 1& 1& 0& 0\\ 0& 0& 0& -1& -1& -1& 0& 1& 0\\ 0& 0& 0& 0& -1& 0& -1& 0& 1\end{array}\right].$$ Equivalently we may write it as $$0\le {\mathbf{A}}_{\mathrm{eq}}\mathbf{x}\le 0.$$ Together with the upper bounds they can be now rewritten into the canonical form $$\underset{\mathbf{A}}{\underbrace{\left[\begin{array}{c}\mathbf{E}\\ {\mathbf{A}}_{\mathrm{eq}}\\ -{\mathbf{A}}_{\mathrm{eq}}\end{array}\right]}}\mathbf{x}\le \underset{\mathbf{b}}{\underbrace{\left[\begin{array}{c}\mathbf{u}\\ 0\\ 0\end{array}\right]}}$$

Code

• python - CVXPY

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}$$

QP Problem Examples

Circular paraboloid with a single constraint

Let's visualize and solve a very simple QP problem of minimizing $x_1$ and $x_2$ for an objective function in the form of a circular paraboloid and a single linear inequality constraint.

Mathematical model

$$\begin{array}{rl}\underset{x_1,x_2}{\min }& x_1^2+x_2^2\\ \text{s.t.}& -2x_1+x_2\le -1\end{array}$$

Canonical form

$$\begin{array}{rl}\mathbf{Q}& =\left[\begin{array}{cc}2& 0\\ 0& 2\end{array}\right]\\ \mathbf{c}& =\left[\begin{array}{c}0\\ 0\end{array}\right]\\ \mathbf{A}& =\left[\begin{array}{cc}-1& 1\end{array}\right]\\ \mathbf{b}& =\left[\begin{array}{c}-1\end{array}\right]\end{array}$$

Code

• julia
  • JuMP
  • OSQP
• python
  • CVXPY
  • OSQP

Economic dispatch

We have three power plants each producing electricity to satisfy the demand of $P_{\Sigma }=3000$ MW. Each of the power plants has a minimum and maximum capacity $P_{i_{\min }}$ and $P_{i_{\max }}$ and a quadratic cost model associated with its power output $l_i=a_i+b_i{P}_i+c_i{P}_i^2$. The capacity limits and cost coefficients are provided in the table bellow.

Mathematical model

$$\begin{array}{rl}\underset{\{P_i{\}}_{i=1}^3}{\min }& \sum _{i=1}^3{a}_i+b_i{P}_i+c_i{P}_i^2\\ \text{s.t.}& \sum _{i=1}^3{P}_i=P_{\Sigma }\\ & P_{i_{\min }}\le P_i\le P_{i_{\min }},i=1,\dots ,3\end{array}$$

OSQP form

$$\begin{array}{rl}\mathbf{P}& =\left[\begin{array}{ccc}{c}_1& 0& 0\\ 0& c_2& 0\\ 0& 0& c_3\end{array}\right]\\ \mathbf{q}& =\left[\begin{array}{c}{b}_1\\ b_2\\ b_3\end{array}\right]\\ \mathbf{A}& =\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\\ 1& 1& 1\end{array}\right]\\ \mathbf{l}& =\left[\begin{array}{c}{P}_{1_{\min }}\\ P_{2_{\min }}\\ P_{3_{\min }}\\ P_{\Sigma }\end{array}\right]\\ \mathbf{u}& =\left[\begin{array}{c}{P}_{1_{\max }}\\ P_{2_{\max }}\\ P_{3_{\max }}\\ P_{\Sigma }\end{array}\right]\end{array}$$

Code

• python - OSQP

Model Predictive Control (MPC)

Optimal control of linear systems on a finite horizon can be more often than not stated as a QP. Formulation of these problems is a topic of future chapters.

NLP Problem Examples

Proportions of a canister

We are tasked with designing the proportions of a 0.5 liter cylindrical canister to minimize its surface area and therefore the amount of material used.

Cylinder's volume and surface are $$\begin{array}{rl}V& =\pi r^{2}h\\ S& =2\pi (r^2+rh).\end{array}$$

Mathematical model

$$\begin{array}{rl}\underset{r,h}{\min }& 2\pi (r^2+rh)\\ \text{s.t.}& \pi r^{2}h=0.5\\ & r\ge 0\\ & h\ge 0\end{array}$$

As we are dealing with a quadratic objective function and a cubic equality constraint, the python package CVXPY does not have an interface to a suitable solver. Fortunately, the julia package JuMP does.

• julia - JuMP

Tree nursery

We are tasked with planting a new patch of forest to offset logging activity. Because of the wildlife living there, the young trees need to be fences off from the rest of the forest until they mature.

As we will be planting adjacent plots in the future, we have decided upon fencing off a rectangular area. The factor limiting its size is that we only have 100 meters of suitable fencing. What is the largest rectangular area that we can plant?

Mathematical model

$$\begin{array}{rl}\underset{x_1,x_2}{\max }& x_1{x}_2\\ \text{s.t.}& 2x_1+2x_2\le 100\end{array}$$

QP-like form

$$\begin{array}{rl}\mathbf{Q}& =\left[\begin{array}{cc}0& -1\\ -1& 0\end{array}\right]\\ \mathbf{c}& =\left[\begin{array}{c}0\\ 0\end{array}\right]\\ \mathbf{A}& =\left[\begin{array}{cc}2& 2\end{array}\right]\\ \mathbf{b}& =\left[\begin{array}{c}100\end{array}\right]\end{array}$$

The reason why this problem falls under nonlinear programming is that Q is not positive semi-definite and therefore the problem is non-convex.

Code

• julia - JuMP

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, applying inputs from the beginning of the predicted horizon. There are many algorithms for trajectory optimization, each adapted for a specific use-case. When talking about nonlinear systems in general, main factors determining the applicability of a specific algorithm are if the system is represented in continuous or discrete time and if its dynamics are continuous or hybrid.

In this course we will limit ourselves to the specific case of linear systems¹ represented in discrete time with a quadratic objective (LQ control problem) and box constraints on the system's inputs ². This specific control problem can be transcribed into a QP problem with specialized solvers dedicated to finding its solution. We will go through two formulations of this problem:

• direct - both states ${\mathbf{x}}_k$ and inputs ${\mathbf{u}}_k$ are the decision variables,
• indirect - 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+1},\{{\mathbf{u}}_k{\}}_{k=0}^N}{\min }& {\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\\ & {\mathbf{u}}_{\min }\le {\mathbf{u}}_k\le {\mathbf{u}}_{\max },\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^{\top }& {\mathbf{x}}_1^{\top }& \dots & \dots & {\mathbf{x}}_{N+1}^{\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}$$

Indirect Linear-Quadratic MPC

$$\begin{array}{rl}\underset{\{{\mathbf{u}}_k{\}}_0^N}{\min }& {\mathbf{x}}_k^{\top }\mathbf{Q}{\mathbf{x}}_k+{\mathbf{u}}_k^{\top }\mathbf{R}{\mathbf{u}}_k\\ \text{s.t.}& {\mathbf{u}}_{\min }\le {\mathbf{u}}_k\le {\mathbf{u}}_{\max },\end{array}$$ where $${\mathbf{x}}_k={\mathbf{A}}^k{\mathbf{x}}_0+\sum _{i=0}^{k-1}{\mathbf{A}}^{k-1-i}\mathbf{B}{\mathbf{u}}_i,k=0,\dots ,N+1$$

Linear-Quadratic MPC Examples

2-DOF double integrator

Let control a 2-DOF double integrator with an MPC controller with a prediction horizon of one second and quadratic running cost penalizing the state-error and input level.

We will use the first order approximation $${\mathbf{x}}_{k+1}=\underset{\mathbf{A}}{\underbrace{(\mathbf{I}+{\mathbf{A}}_{c}h)}}{\mathbf{x}}_k+\underset{\mathbf{B}}{\underbrace{{\mathbf{B}}_{c}h}}{\mathbf{u}}_k$$ with a timestep of $10^{-2}$s to discretize its continuous dynamics $$\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x},\mathbf{u})=\underset{{\mathbf{A}}_c}{\underbrace{\left[\begin{array}{cccc}0& 0& 1& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]}}\mathbf{x}+\underset{{\mathbf{B}}_c}{\underbrace{\left[\begin{array}{cc}0& 0\\ 0& 0\\ 1& 0\\ 0& 1\end{array}\right]}}\mathbf{u}.$$

We will formulate the running cost as $$l({\mathbf{x}}_k,{\mathbf{u}}_k)={\mathbf{x}}_k^{\top }\mathbf{Q}{\mathbf{x}}_k+{\mathbf{u}}_k^{\top }\mathbf{R}{\mathbf{u}}_k$$ and place additional constraints $$-2\le {\mathbf{u}}_k\le 2$$ on the controllers inputs.

Code

• Direct
  • julia
    • JuMP
    • OSQP
  • python
    • CVXPY
    • OSQP

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.

Discretization via integration using the explicit Euler scheme

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 scheme $$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}$$