Model Predictive Control Basics

A hands-on tutorial with Python and CasADi

Quick Summary

In this article we will:

• Cover the basic ideas.

• Code up a solver in Python. (Get all the code of this article here)

• Play with a simple linear system: the double integrator.

---

1. Introduction

Model predictive control (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.

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 capture the true plant’s behaviour 100%, 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 resolve the OCP. This closes the loop and creates a feedback.

The maths behind it is relatively simple and intuitive (especially when compared to things like robust control) 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 a optimization problems “online” in real time, which can be a problem if you’re controlling a fast system or have limited computing resources.

1.2 Running Example

Throughout the article I will 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. Here \( x_1(t) \in \mathbb{R} \) is the position whereas \( x_2(t) \in \mathbb{R} \) is the velocity. You can think of this system as a 1kg block sliding on a frictionless table, with \(u(t)\) the applied force.

If we constrain the control to be piecewise constant over intervals of length 0.1 seconds, we get the discrete-time system:

\(\begin{equation*} x_k = Ax_k + B u_k, \end{equation*}\)

with \(k \in \mathbb{Z}\), where,

\(\begin{align*} A \triangleq \left(\begin{array}{cc} 1 & 0.1\\ 0 & 1 \end{array} \right) ,\quad 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:

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):

\(\newcommand{\u}{\mathbf{u}} \newcommand{\x}{\mathbf{x}} \newcommand{\mbbR}{\mathbb{R}} \newcommand{\OCP}{\mathrm{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\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 will let,

• \(\mathbf{u}\triangleq (u_0,u_1,\dots,u_{K-1})\in\mathbb{R}^{K}\) denote the control sequence,

• \(\mathbf{x}\triangleq (x_0,x_1,\dots,x_K)\in\mathbb{R}^{2(K+1)}\) denote the state sequence.

To be rigorous, we’ll say that the pair \( (\u^{\star}, \x^{\star}) \in \mathbb{R}^{K} \times \mathbb{R}^{2(K+1)} \) 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,

\(\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*}\)

and \( \Omega \) denotes all admissible pairs,

\(\Omega \triangleq \{(\u,\x)\in \mathbb{R}^{K}\times \mathbb{R}^{2(K+1)} : (1)-(4)\,\, \mathrm{hold}\}.\)

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.

2.1 Coding an OCP solver

CasADi’s opti stack makes it really easy to set up and solve the OCP.

First, some preliminaries:

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:

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:

# 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,

# 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:

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

from MPC_tutorial import plot_solution

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

3. Model Predictive Control

The solution to \( \OCP(\hat \x) \), \( (\x^{\star},\u^{\star}) \), for a given \(\hat \x\), 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 basic 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} \)

3.1 Coding the MPC algorithm

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

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:

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.

plot_solution(x_cl, u_cl)

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.

Here’s a plot of the closed loop from a bunch of 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 substack 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.