Abstract

This article covers the basic ideas behind Model Predictive Control (MPC). We will code up a solver in Python and play with a simple linear system (the double integrator). Get all the code from this repo.

1 Introduction

MPC is a popular feedback control methodology where a finite-horizon optimal control problem (OCP) is iteratively solved with an updated measured state on each iteration.

Figure 1: Fundamental idea of MPC

In the OCP one uses a model of the plant to find the optimal open loop control over the finite horizon considered. Because the model cannot fully capture the true plant’s behaviour, and because in the real world the system is subjected to noise and disturbances, one only applies the first portion of the optimal open loop control, and regularly measures the state to re-solve the OCP. This closes the loop and creates a feedback.

The underlying maths is relatively simple and it is easy to code up an MPC controller. Other pros are that it can effectively handle hard and soft constraints on the state and control (hard constraints are strict, whereas soft constraints are enforced via penalisation in the cost function) and it can generally be used on nonlinear systems with nonconvex constraints (depending on how nasty these are of course!). The only con is that you need to solve an optimization problems “online” in real time, which can be a problem if you’re controlling a fast system or have limited computing resources.

1.1 Running Example

Throughout the article we’ll consider the double integrator with a zero-order hold control (i.e. a piecewise constant control) as the running example in the code. The continuous time system reads: \[ \begin{align*} \dot x_1(t) & = x_2(t), \\ \dot x_2(t) & = u(t), \end{align*} \] with $t \in \mathbb{R}$ denoting time. You can think of this system as a 1kg block sliding on a frictionless table, with $x_1(t) \in \mathbb{R}$ being its position, $x_2(t) \in \mathbb{R}$ its velocity and $u(t)$ the applied force, see Figure 2.

If we constrain the control to be piecewise constant over constant intervals of length 0.1 seconds, we get the discrete-time system: \[ \begin{align*} x_{k + 1} & = Ax_k + Bu_k, \end{align*} \]

with $k \in \mathbb{Z}$, where, \[\begin{align*} A \triangleq \left( \begin{array}{cc} 1 & 0.1 \\ 0 & 1 \end{array} \right), \,\, B \triangleq \left( \begin{array}{c} 0.05 \\ 1 \end{array} \right), \end{align*}\] and $x_k \in \mathbb{R}^2$, $u_k \in \mathbb{R}$. (Note that I use $x_k$ and $u_k$ to refer to the discrete-time system’s state and control to make the notation neater in the rest of the article.) You can use the scipy packages’s cont2discrete function to get this discrete time system, as follows:

Code

import numpy as np
from scipy.signal import cont2discrete

A = np.array([[0, 1],[0, 0]])
B = np.array([[0],[1]])
C = np.array([[1, 0],[0, 1]])
D = np.array([[0, 0],[0, 0]])
dt = 0.1 # in seconds
discrete_system = cont2discrete((A, B, C, D), dt, method='zoh')
A_discrete, B_discrete, *_ = discrete_system

2 Optimal Control Problem

We’ll consider the following discrete-time optimal control problem (OCP): \[ \mathrm{OCP}(\hat x): \begin{cases} \min\limits_{\mathbf{u},\mathbf{x}} \quad & \sum_{k=0}^{K-1} \left(x_k^{\top}Qx_k + u_k^\top R u_k \right)+ x_{K}^\top Q_K x_{K}\\ \mathrm{subject\ to:}\quad & x_{k+1} = Ax_k + Bu_k, &k \in[0:K-1], & \dots(1)\\ \quad & x_0 = \hat x, & & \dots(2) \\ \quad & x_k\in [-1,1]\times(-\infty,\infty),& k \in[1:K], & \dots(3)\\ \quad & u_k\in [-1,1],& k \in[0:K-1], & \dots(4) \end{cases} \]

where,

$K \in\mathbb{R}_{\geq 0}$ denotes the finite horizon over which we solve the OCP,
$k \in \mathbb{Z}$ denotes the discrete time step,
$[p : q]$, with $p,q\in\mathbb{Z}$, denotes the set of integers $\{p,p+1,\dots,q\}$,
$\hat{x}\in\mathbb{R}^2$ denotes the initial condition of the dynamical system,
$x_k\in\mathbb{R}^2$ denotes the state at step $k$,
$u_k\in\mathbb{R}$ denotes the control at step $k$,
$Q\in\mathbb{R}^{2\times 2}$, $R\in\mathbb{R}$ and $Q_K \in \mathbb{R}^{2\times 2}$ are square positive definite matrices that specify the cost function ($R$ is scalar here because $u$ is scalar).

Moreover, we let,

$\mathbf{u}\triangleq (u_0,u_1,\dots,u_{K-1})\in\mathbb{R}^{K}$ denote the control sequence, and
$\mathbf{x}\triangleq (x_0,x_1,\dots,x_K)\in\mathbb{R}^{2(K+1)}$ denote the state sequence.

To be rigorous, let $\Omega$ denote all admissible pairs, \[\begin{equation*} \Omega \triangleq \{(\mathbf{u},\mathbf{x})\in \mathbb{R}^{K}\times \mathbb{R}^{2(K+1)} : (1)-(4)\,\, \mathrm{hold}\}. \end{equation*}\] We’ll say that the admissible pair $(\mathbf{u}^{\star}, \mathbf{x}^{\star}) \in \Omega$ is a solution to $\mathrm{OCP}(\hat x)$ provided that it minimises the cost function over all admissible pairs, that is, \[\begin{equation*} J(\mathbf{u}^{\star}, \mathbf{x}^{\star}) \leq J(\mathbf{u}, \mathbf{x}),\quad \forall (\mathbf{u},\mathbf{x})\in \Omega, \end{equation*}\] where $J:\mathbb{R}^K \times \mathbb{R}^{2(K+1)}\rightarrow \mathbb{R}_{\geq 0}$ is defined by \[\begin{equation*} J(\mathbf{u},\mathbf{x}) \triangleq \left( \sum_{k=0}^{K-1 }x_k^\top Q x_k + u_k^\top R u_k \right) + x_K^\top Q_K x_K. \end{equation*}\] Thus, the optimal control problem is to find a control and state sequence, $(\mathbf{u},\mathbf{x})$, that minimises the cost function, subject to the dynamics, $f$, as well as constraints on the state and control, $x_k\in[-1,1]\times(-\infty,\infty)$, $u_k \in [-1,1]$, for all $k$. The cost function is vital to the controller’s performance. Not only in the sense of ensuring the controller behaves well (for example, to prevent erratic signals) but also in specifying the equilibrium point the closed loop state will settle at. More on this in Section 4. Note how $\mathrm{OCP}(\hat x)$ is parametrised with respect to the initial state, $\hat x$. This comes from the fundamental idea behind MPC: that the optimal control problem is iteratively solved with an updated measured state.

2.1 Coding an OCP solver

CasADi’s opti stack makes it really easy to set up and solve the OCP. First, some preliminaries:

Code

from casadi import *

n = 2 # state dimension
m = 1 # control dimension
K = 100 # prediction horizon

# an arbitrary initial state
x_hat = np.array([[0.5],[0.5]]) # 2 x 1 vector

# Linear cost matrices (we'll just use identities)
Q = np.array([[1. , 0],
            [0. , 1. ]])
R = np.array([[1]])
Q_K = Q

# Constraints for all k
u_max = 1
x_1_max = 1
x_1_min = -1

Now we define the problem’s decision variables:

Code

opti = Opti()

x_tot = opti.variable(n, K+1)  # State trajectory
u_tot = opti.variable(m, K)    # Control trajectory

Next, we impose the dynamic constraints and set up the cost function:

Code

# Specify the initial condition
opti.subject_to(x_tot[:, 0] == x_hat)

cost = 0
for k in range(K):
    # add dynamic constraints
    x_tot_next = get_x_next_linear(x_tot[:, k], u_tot[:, k])
    opti.subject_to(x_tot[:, k+1] == x_tot_next)

    # add to the cost
    cost += mtimes([x_tot[:,k].T, Q, x_tot[:,k]]) + \     
                           mtimes([u_tot[:,k].T, R, u_tot[:,k]])

# terminal cost
cost += mtimes([x_tot[:,K].T, Q_K, x_tot[:,K]])
def get_x_next_linear(x, u):
    # Linear system
    A = np.array([[1. , 0.1],
                [0. , 1. ]])
    B = np.array([[0.005],
                  [0.1  ]])
    
    return mtimes(A, x) + mtimes(B, u)

The code mtimes([x_tot[:,k].T, Q, x_tot[:,k]]) implements matrix multiplication, $x_k^\top Q x_k$.

We now add the control and state constraints,

Code

# constrain the control
opti.subject_to(opti.bounded(-u_max, u_tot, u_max))

# constrain the position only
opti.subject_to(opti.bounded(x_1_min, x_tot[0,:], x_1_max))

and solve:

Code

# Say we want to minimise the cost and specify the solver (ipopt)
opts = {"ipopt.print_level": 0, "print_time": 0}
opti.minimize(cost)
opti.solver("ipopt", opts)
    
solution = opti.solve()

# Get solution
x_opt = solution.value(x_tot)
u_opt = solution.value(u_tot)

We can plot the solution with the repo’s plot_solution() function, see Figure 3

Code

from MPC_tutorial import plot_solution

plot_solution(x_opt, u_opt.reshape(1,-1)) # must reshape u_opt to (1,K)

Figure 3: Solution to the OCP with $\hat{\mathbf{x}} = (\frac{1}{2},\frac{1}{2})^\top$.

3 Model Predictive Control

The solution to $\mathrm{OCP}(\hat{\mathbf{x}})$ for a given $\hat{\mathbf{x}}$, namely $(\mathbf{x}^{\star},\mathbf{u}^{\star})$, provides us with an open loop control, $\mathbf{u}^{\star}$. We now close the loop by iteratively solving $\mathrm{OCP}(\hat{\mathbf{x}})$ and updating the initial state (this is the MPC algorithm).

\[\begin{aligned} &\textbf{Input:} \quad \mathbf{x}^{\mathrm{init}}\in\mathbb{R}^2 \\ &\quad \hat{\mathbf{x}} \gets \mathbf{x}^{\mathrm{init}} \\ &\textbf{for } k \in [0:\infty) \textbf{:} \\ &\quad (\mathbf{x}^{\star}, \mathbf{u}^{\star}) \gets \arg\min \mathrm{OCP}(\hat{\mathbf{x}})\\ &\quad \mathrm{apply}\ u_0^{\star} \mathrm{\ to\ the\ system} \\ &\quad \hat{\mathbf{x}} \gets \mathrm{measured \ state\ at\ } k+1 \\ &\textbf{end for} \end{aligned}\]

3.1 Coding the MPC algorithm

The rest is quite straightforward. First, I’ll put all our previous code in a function:

Code

def solve_OCP(x_hat, K):
    ....

    return x_opt, u_opt

Note that I’ve parametrised the function with the initial state, $\hat{\mathbf{x}}$, as well as the prediction horizon, $K$. The MPC loop is then implemented with:

Code

x_init = np.array([[0.5],[0.5]]) # 2 x 1 vector
K = 10
number_of_iterations = 150 # must of course be finite!

# matrices of zeros with the correct sizes to store the closed loop
u_cl = np.zeros((1, number_of_iterations))
x_cl = np.zeros((2, number_of_iterations + 1))

x_cl[:, 0] = x_init[:, 0]

x_hat = x_init
for i in range(number_of_iterations):
    _, u_opt = solve_OCP(x_hat, K)
    u_opt_first_element = u_opt[0]

    # save closed loop x and u
    u_cl[:, i] = u_opt_first_element
    x_cl[:, i+1] = np.squeeze(get_x_next_linear(x_hat,   
                                                u_opt_first_element))

    # update initial state
    x_hat = get_x_next_linear(x_hat, u_opt_first_element)

Again, we can plot the closed loop solution, see Figure 4.

Code

plot_solution(x_cl, u_cl)

Figure 4: The MPC closed loop, initiating from $\mathbf{x}^{\mathrm{init}}=(\frac{1}{2}, \frac{1}{2})^\top$

Note that I’ve “measured” the plant’s state by using the function get_x_next_linear(). In other words, I’ve assumed that our model is 100% correct. Figure 5 shows the closed loop from a bunch of initial states.

Figure 5: The MPC closed loop from various initial states.

4 Further topics

4.1 Stability and Recursive Feasibility

Two of the most important aspects of an MPC controller are recursive feasibility of the iteratively invoked OCP and stability of the closed loop. In other words, if I have solved the OCP at time $k$, will there exist a solution to the OCP at time $k+1$? If there exists a solution to the OCP at every time step, will the closed-loop state asymptotically settle at an equilibrium point (i.e. will it be stable)?

Ensuring that an MPC controller exhibits these two properties involves carefully designing the cost function and constraints, and choosing a long enough prediction horizon. Going back to our example, recall that the matrices in the cost function were simply chosen to be:

\[ Q = \left( \begin{array}{cc} 1 & 0\\ 0 & 1 \end{array} \right),\, Q_K = \left( \begin{array}{cc} 1 & 0\\ 0 & 1 \end{array} \right),\, R = 1. \]

In other words, the OCP penalises the distance of the state to the origin and thus drives it there. As you can probably guess, if the prediction horizon, $K$, is very short and the initial state is located very close to the constraints at $x_1=\pm 1$, the OCP will find solutions with insufficient “foresight” and the problem will be infeasible at some future iteration of the MPC loop. (You can also experiment with this by making $K$ small in the code.)

4.2 Some Other Topics

MPC is an active field of research and there are many interesting topics to explore.

What if the full state cannot be measured? This relates to observability and output MPC. What if I do not care about asymptotic stability? This (often) has to do with economic MPC. How do I make the controller robust to noise and disturbances? There are a few ways to deal with this, with tube MPC probably being the best-known.

Future articles might focus on some of these topics.

5 Further reading

Here are some standard and very good textbooks on MPC.

[1] Grüne, L., & Pannek, J. (2016). Nonlinear model predictive control.

[2] Rawlings, J. B., Mayne, D. Q., & Diehl, M. (2020). Model predictive control: Theory, computation, and design.

[3] Kouvaritakis, B., & Cannon, M. (2016). Model predictive control: Classic, robust and stochastic.

--- title: "Model Predictive Control Basics" subtitle: "A Hands-on Tutorial with Python and CasADi" date: 09-13-2025 author: "Willem Esterhuizen" # title-block-banner: "LightSlateGray" title-block-style: "default" format: html: code-fold: show code-tools: true number-sections: true theme: flatly jupyter: python3 abstract: | This article covers the basic ideas behind Model Predictive Control (MPC). We will code up a solver in Python and play with a simple linear system (the double integrator). Get all the code from *[this repo](https://github.com/willem-daniel-esterhuizen/MPC_tutorial)*. categories: [optimal control, model predictive control, tutorial] keywords: - optimal control - model predictive control - tutorial image: many_inits.svg --- \newcommand{\u}{\mathbf{u}} \newcommand{\x}{\mathbf{x}} \newcommand{\mbbR}{\mathbb{R}} \newcommand{\OCP}{\mathrm{OCP}} # Introduction {#sec-MPC} MPC is a popular feedback control methodology where a finite-horizon optimal control problem (OCP) is iteratively solved with an updated measured state on each iteration. <div style="text-align: center;"> ![Fundamental idea of MPC](mpc_flowchart.svg){#fig-mpc-closed-loop width=70%} </div> In the OCP one uses a model of the plant to find the optimal open loop control over the finite horizon considered. Because the model cannot fully capture the true plant’s behaviour, and because in the real world the system is subjected to noise and disturbances, one only applies the first portion of the optimal open loop control, and regularly measures the state to re-solve the OCP. This closes the loop and creates a *feedback*. The underlying maths is relatively simple and it is easy to code up an MPC controller. Other pros are that it can effectively handle hard and soft constraints on the state and control (hard constraints are strict, whereas soft constraints are enforced via penalisation in the cost function) and it can generally be used on nonlinear systems with nonconvex constraints (depending on how nasty these are of course!). The only con is that you need to solve an optimization problems “online” in real time, which can be a problem if you’re controlling a fast system or have limited computing resources. ## Running Example {#sec-running-example} Throughout the article we'll consider the double integrator with a zero-order hold control (i.e. a piecewise constant control) as the running example in the code. The continuous time system reads: $$ \begin{align*} \dot x_1(t) & = x_2(t), \\ \dot x_2(t) & = u(t), \end{align*} $$ with $t \in \mathbb{R}$ denoting time. You can think of this system as a 1kg block sliding on a frictionless table, with $x_1(t) \in \mathbb{R}$ being its position, $x_2(t) \in \mathbb{R}$ its velocity and $u(t)$ the applied force, see @fig-block_on_table. <div style="text-align: center;"> ![The double integrator.](bloack_on_table.svg){#fig-block_on_table width=40%} </div> If we constrain the control to be piecewise constant over constant intervals of length 0.1 seconds, we get the discrete-time system: $$ \begin{align*} x_{k + 1} & = Ax_k + Bu_k, \end{align*} $$ with $k \in \mathbb{Z}$, where, \begin{align*} A \triangleq \left( \begin{array}{cc} 1 & 0.1 \\ 0 & 1 \end{array} \right), \,\, B \triangleq \left( \begin{array}{c} 0.05 \\ 1 \end{array} \right), \end{align*} and $x_k \in \mathbb{R}^2$, $u_k \in \mathbb{R}$. (Note that I use $x_k$ and $u_k$ to refer to the discrete-time system’s state and control to make the notation neater in the rest of the article.) You can use the `scipy` packages’s `cont2discrete` function to get this discrete time system, as follows: ```{python} #| eval: false import numpy as np from scipy.signal import cont2discrete A = np.array([[0, 1],[0, 0]]) B = np.array([[0],[1]]) C = np.array([[1, 0],[0, 1]]) D = np.array([[0, 0],[0, 0]]) dt = 0.1 # in seconds discrete_system = cont2discrete((A, B, C, D), dt, method='zoh') A_discrete, B_discrete, *_ = discrete_system ``` # Optimal Control Problem {#sec-optimal-control-problem} We’ll consider the following discrete-time optimal control problem (OCP): $$ \OCP(\hat x): \begin{cases} \min\limits_{\u,\x} \quad & \sum_{k=0}^{K-1} \left(x_k^{\top}Qx_k + u_k^\top R u_k \right)+ x_{K}^\top Q_K x_{K}\\ \mathrm{subject\ to:}\quad & x_{k+1} = Ax_k + Bu_k, &k \in[0:K-1], & \dots(1)\\ \quad & x_0 = \hat x, & & \dots(2) \\ \quad & x_k\in [-1,1]\times(-\infty,\infty),& k \in[1:K], & \dots(3)\\ \quad & u_k\in [-1,1],& k \in[0:K-1], & \dots(4) \end{cases} $$ where, - $K \in\mathbb{R}_{\geq 0}$ denotes the finite horizon over which we solve the OCP, - $k \in \mathbb{Z}$ denotes the discrete time step, - $[p : q]$, with $p,q\in\mathbb{Z}$, denotes the set of integers $\{p,p+1,\dots,q\}$, - $\hat{x}\in\mathbb{R}^2$ denotes the initial condition of the dynamical system, - $x_k\in\mathbb{R}^2$ denotes the state at step $k$, - $u_k\in\mathbb{R}$ denotes the control at step $k$, - $Q\in\mathbb{R}^{2\times 2}$, $R\in\mathbb{R}$ and $Q_K \in \mathbb{R}^{2\times 2}$ are square positive definite matrices that specify the cost function ($R$ is scalar here because $u$ is scalar). Moreover, we let, - $\mathbf{u}\triangleq (u_0,u_1,\dots,u_{K-1})\in\mathbb{R}^{K}$ denote the control sequence, and - $\mathbf{x}\triangleq (x_0,x_1,\dots,x_K)\in\mathbb{R}^{2(K+1)}$ denote the state sequence. To be rigorous, let $\Omega$ denote all *admissible pairs*, \begin{equation*} \Omega \triangleq \{(\u,\x)\in \mathbb{R}^{K}\times \mathbb{R}^{2(K+1)} : (1)-(4)\,\, \mathrm{hold}\}. \end{equation*} We’ll say that the admissible pair $(\u^{\star}, \x^{\star}) \in \Omega$ is a *solution* to $\OCP(\hat x)$ provided that it minimises the cost function over all admissible pairs, that is, \begin{equation*} J(\u^{\star}, \x^{\star}) \leq J(\u, \x),\quad \forall (\u,\x)\in \Omega, \end{equation*} where $J:\mathbb{R}^K \times \mathbb{R}^{2(K+1)}\rightarrow \mathbb{R}_{\geq 0}$ is defined by \begin{equation*} J(\u,\x) \triangleq \left( \sum_{k=0}^{K-1 }x_k^\top Q x_k + u_k^\top R u_k \right) + x_K^\top Q_K x_K. \end{equation*} Thus, the optimal control problem is to find a control and state sequence, $(\u,\x)$, that minimises the cost function, subject to the dynamics, $f$, as well as constraints on the state and control, $x_k\in[-1,1]\times(-\infty,\infty)$, $u_k \in [-1,1]$, for all $k$. The cost function is vital to the controller’s performance. Not only in the sense of ensuring the controller behaves well (for example, to prevent erratic signals) but also in specifying the equilibrium point the closed loop state will settle at. More on this in Section 4. Note how $\OCP(\hat x)$ is parametrised with respect to the initial state, $\hat x$. This comes from the fundamental idea behind MPC: that the optimal control problem is iteratively solved with an updated measured state. ## Coding an OCP solver CasADi’s opti stack makes it really easy to set up and solve the OCP. First, some preliminaries: ```{python} #| eval: false from casadi import * n = 2 # state dimension m = 1 # control dimension K = 100 # prediction horizon # an arbitrary initial state x_hat = np.array([[0.5],[0.5]]) # 2 x 1 vector # Linear cost matrices (we'll just use identities) Q = np.array([[1. , 0], [0. , 1. ]]) R = np.array([[1]]) Q_K = Q # Constraints for all k u_max = 1 x_1_max = 1 x_1_min = -1 ``` Now we define the problem’s decision variables: ```{python} #| eval: false opti = Opti() x_tot = opti.variable(n, K+1) # State trajectory u_tot = opti.variable(m, K) # Control trajectory ``` Next, we impose the dynamic constraints and set up the cost function: ```{python} #| eval: false # Specify the initial condition opti.subject_to(x_tot[:, 0] == x_hat) cost = 0 for k in range(K): # add dynamic constraints x_tot_next = get_x_next_linear(x_tot[:, k], u_tot[:, k]) opti.subject_to(x_tot[:, k+1] == x_tot_next) # add to the cost cost += mtimes([x_tot[:,k].T, Q, x_tot[:,k]]) + \ mtimes([u_tot[:,k].T, R, u_tot[:,k]]) # terminal cost cost += mtimes([x_tot[:,K].T, Q_K, x_tot[:,K]]) def get_x_next_linear(x, u): # Linear system A = np.array([[1. , 0.1], [0. , 1. ]]) B = np.array([[0.005], [0.1 ]]) return mtimes(A, x) + mtimes(B, u) ``` The code `mtimes([x_tot[:,k].T, Q, x_tot[:,k]])` implements matrix multiplication, $x_k^\top Q x_k$. We now add the control and state constraints, ```{python} #| eval: false # constrain the control opti.subject_to(opti.bounded(-u_max, u_tot, u_max)) # constrain the position only opti.subject_to(opti.bounded(x_1_min, x_tot[0,:], x_1_max)) ``` and solve: ```{python} #| eval: false # Say we want to minimise the cost and specify the solver (ipopt) opts = {"ipopt.print_level": 0, "print_time": 0} opti.minimize(cost) opti.solver("ipopt", opts) solution = opti.solve() # Get solution x_opt = solution.value(x_tot) u_opt = solution.value(u_tot) ``` We can plot the solution with the [repo's](https://github.com/willem-daniel-esterhuizen/MPC_tutorial) `plot_solution()` function, see @fig-ocp-open-loop ```{python} #| eval: false from MPC_tutorial import plot_solution plot_solution(x_opt, u_opt.reshape(1,-1)) # must reshape u_opt to (1,K) ``` <div style="text-align: center;"> ![Solution to the OCP with $\hat{\x} = (\frac{1}{2},\frac{1}{2})^\top$.](ocp-open-loop.svg){#fig-ocp-open-loop} </div> # Model Predictive Control {#sec-model-predictive-control} The solution to $\OCP(\hat{\x})$ for a given $\hat{\x}$, namely $(\x^{\star},\u^{\star})$, provides us with an open loop control, $\u^{\star}$. We now *close the loop* by iteratively solving $\OCP(\hat{\x})$ and updating the initial state (this is the MPC algorithm). \begin{aligned} &\textbf{Input:} \quad \x^{\mathrm{init}}\in\mathbb{R}^2 \\ &\quad \hat{\x} \gets \x^{\mathrm{init}} \\ &\textbf{for } k \in [0:\infty) \textbf{:} \\ &\quad (\x^{\star}, \u^{\star}) \gets \arg\min \OCP(\hat{\x})\\ &\quad \mathrm{apply}\ u_0^{\star} \mathrm{\ to\ the\ system} \\ &\quad \hat{\x} \gets \mathrm{measured \ state\ at\ } k+1 \\ &\textbf{end for} \end{aligned} ## Coding the MPC algorithm The rest is quite straightforward. First, I’ll put all our previous code in a function: ```{python} #| eval: false def solve_OCP(x_hat, K): .... return x_opt, u_opt ``` Note that I’ve parametrised the function with the initial state, $\hat{\x}$, as well as the prediction horizon, $K$. The MPC loop is then implemented with: ```{python} #| eval: false x_init = np.array([[0.5],[0.5]]) # 2 x 1 vector K = 10 number_of_iterations = 150 # must of course be finite! # matrices of zeros with the correct sizes to store the closed loop u_cl = np.zeros((1, number_of_iterations)) x_cl = np.zeros((2, number_of_iterations + 1)) x_cl[:, 0] = x_init[:, 0] x_hat = x_init for i in range(number_of_iterations): _, u_opt = solve_OCP(x_hat, K) u_opt_first_element = u_opt[0] # save closed loop x and u u_cl[:, i] = u_opt_first_element x_cl[:, i+1] = np.squeeze(get_x_next_linear(x_hat, u_opt_first_element)) # update initial state x_hat = get_x_next_linear(x_hat, u_opt_first_element) ``` Again, we can plot the *closed loop* solution, see @fig-mpc-closed-loop. ```{python} #| eval: false plot_solution(x_cl, u_cl) ``` <div style="text-align: center;"> ![The MPC closed loop, initiating from $\x^{\mathrm{init}}=(\frac{1}{2}, \frac{1}{2})^\top$](mpc-closed-loop.svg){#fig-mpc-closed-loop} </div> Note that I’ve “measured” the plant’s state by using the function `get_x_next_linear()`. In other words, I’ve assumed that our model is 100% correct. @fig-many_inits shows the closed loop from a bunch of initial states. <div style="text-align: center;"> ![The MPC closed loop from various initial states.](many_inits.svg){#fig-many_inits} </div>  # Further topics ## Stability and Recursive Feasibility Two of the most important aspects of an MPC controller are *recursive feasibility* of the iteratively invoked OCP and *stability* of the closed loop. In other words, if I have solved the OCP at time $k$, will there exist a solution to the OCP at time $k+1$? If there exists a solution to the OCP at every time step, will the closed-loop state asymptotically settle at an equilibrium point (i.e. will it be stable)? Ensuring that an MPC controller exhibits these two properties involves carefully designing the cost function and constraints, and choosing a long enough prediction horizon. Going back to our example, recall that the matrices in the cost function were simply chosen to be: $$ Q = \left( \begin{array}{cc} 1 & 0\\ 0 & 1 \end{array} \right),\, Q_K = \left( \begin{array}{cc} 1 & 0\\ 0 & 1 \end{array} \right),\, R = 1. $$ In other words, the OCP penalises the distance of the state to the origin and thus drives it there. As you can probably guess, if the prediction horizon, $K$, is very short and the initial state is located very close to the constraints at $x_1=\pm 1$, the OCP will find solutions with insufficient “foresight” and the problem will be infeasible at some future iteration of the MPC loop. (You can also experiment with this by making $K$ small in the code.) ## Some Other Topics MPC is an active field of research and there are many interesting topics to explore. What if the full state cannot be measured? This relates to *observability* and *output MPC*. What if I do not care about asymptotic stability? This (often) has to do with *economic MPC*. How do I make the controller robust to noise and disturbances? There are a few ways to deal with this, with *tube MPC* probably being the best-known. Future articles might focus on some of these topics. # Further reading Here are some standard and very good textbooks on MPC. [1] Grüne, L., & Pannek, J. (2016). *Nonlinear model predictive control*. [2] Rawlings, J. B., Mayne, D. Q., & Diehl, M. (2020). *Model predictive control: Theory, computation, and design*. [3] Kouvaritakis, B., & Cannon, M. (2016). *Model predictive control: Classic, robust and stochastic*.