## Model Predictive Control Example

Consider the following model predictive control [1], [2] example. To determine the optimal control input $u \in \mathbb{R}^m$ applied to the linear time-invariant (LTI) system $\left( A, B\right) \in \left( \mathbb{R}^{n \times n}, \mathbb{R}^{n \times m} \right)$ with state $x \in \mathbb{R}^n$, we solve the optimization problem

\begin{equation}
\begin{array}{ll}
\text{minimize} \quad & x_H^T P x_H + \sum_{i=0}^{H-1} x_i^T Q x_i + u_i^T R u_i\\
\text{subject to} \quad &x_{i+1} = A x_i + B u_i \quad \forall i \in \{0, ..., H-1\} \\
& \Vert u_i \Vert_\infty \leq 1 \quad \forall i \in \{0, ..., H-1\} \\
& x_0 = x_\mathrm{init}, \\
\end{array}
\end{equation}

where $x_i \in \mathbb{R}^n$ and $u_i \in \mathbb{R}^m$ are the variables. The prediction horizon is $H$, the control input is constrained within a box of size $1$ and the cost matrices are positive definite: $P, Q, R \in \mathbb{S}_{++}^n, \mathbb{S}_{++}^n, \mathbb{S}_{++}^m$. Usually, $P$ is chosen as the solution to the discrete-time algebraic Riccati equation for the given LTI system and cost matrices $Q, R$. The initial measured state is $x_\mathrm{init}$. We arrange the state and input variables to matrices $X$ and $U$ with $X_{:,i} = x_i$ and $U_{:,i} = u_i$, respectively, and reformulate the problem to be [DPP-compliant](https://www.cvxpy.org/tutorial/advanced/index.html#disciplined-parametrized-programming), i.e.,

\begin{equation}
\begin{array}{ll}
\text{minimize} \quad & \Vert P^{1/2} X_{:,H} \Vert_2^2 + \Vert Q^{1/2} X_{:,0:H-1} \Vert_F^2 + \Vert R^{1/2} U \Vert_F^2\\
\text{subject to} \quad &X_{:,1:H} = A X_{:,0:H-1} + B U \\
& | U | \leq \mathbb{1} \\
& X_{:,0} = x_\mathrm{init}, \\
\end{array}
\end{equation}

with variables $X \in \mathbb{R}^{n \times H+1}$ and $U \in \mathbb{R}^{m \times H}$. Note that $|U|$ denotes the element-wise absolute value of $U$.

Let's define the corresponding CVXPY problem.


```python
import cvxpy as cp

# define dimensions
H, n, m = 10, 6, 3

# define variables
U = cp.Variable((m, H), name='U')
X = cp.Variable((n, H+1), name='X')

# define parameters
Psqrt = cp.Parameter((n, n), name='Psqrt')
Qsqrt = cp.Parameter((n, n), name='Qsqrt')
Rsqrt = cp.Parameter((m, m), name='Rsqrt')
A = cp.Parameter((n, n), name='A')
B = cp.Parameter((n, m), name='B')
x_init = cp.Parameter(n, name='x_init')

# define objective
objective = cp.Minimize(cp.sum_squares(Psqrt@X[:,H-1]) + cp.sum_squares(Qsqrt@X[:,:H]) + cp.sum_squares(Rsqrt@U))

# define constraints
constraints = [X[:,1:] == A@X[:,:H]+B@U,
               cp.abs(U) <= 1,
               X[:,0] == x_init]

# define problem
problem = cp.Problem(objective, constraints)
```

Assign parameter values and solve the problem. In this case, the state $x = \left[p^T v^T\right]^T$ consists of position $p \in \mathbb{R}^3$ and velocity $v \in \mathbb{R}^3$ of some rigid body in three-dimensional space. The control input $u$ is the force vector that acts on the body's center of mass and its rotational dynamics is not considered. The discretization step is denoted by $t_d \in \mathbb{R}_{++}$.


```python
import numpy as np

# continuous-time dynmaics
A_cont = np.concatenate((np.array([[0, 0, 0, 1, 0, 0],
                                   [0, 0, 0, 0, 1, 0],
                                   [0, 0, 0, 0, 0, 1]]),
                         np.zeros((3,6))), axis=0)
mass = 1
B_cont = np.concatenate((np.zeros((3,3)), 
                         (1/mass)*np.diag(np.ones(3))), axis=0)

# discrete-time dynamics
td = 0.1
A.value = np.eye(n)+td*A_cont
B.value = td*B_cont

# cost
Psqrt.value = np.eye(n)
Qsqrt.value = np.eye(n)
Rsqrt.value = np.sqrt(0.1)*np.eye(m)

# measurement
x_init.value = np.array([2, 2, 2, -1, -1, 1])

val = problem.solve()
```

Generating C source for the problem is as easy as:


```python
from cvxpygen import cpg

cpg.generate_code(problem, code_dir='MPC_code')
```

Now, you can use a python wrapper around the generated code as a custom CVXPY solve method.


```python
from MPC_code.cpg_solver import cpg_solve
import numpy as np
import pickle
import time

# load the serialized problem formulation
with open('MPC_code/problem.pickle', 'rb') as f:
    prob = pickle.load(f)

# assign parameter values
prob.param_dict['A'].value = np.eye(n)+td*A_cont
prob.param_dict['B'].value = td*B_cont
prob.param_dict['Psqrt'].value = np.eye(n)
prob.param_dict['Qsqrt'].value = np.eye(n)
prob.param_dict['Rsqrt'].value = np.sqrt(0.1)*np.eye(m)
prob.param_dict['x_init'].value = np.array([2, 2, 2, -1, -1, 1])

# solve problem conventionally
t0 = time.time()
# CVXPY chooses eps_abs=eps_rel=1e-5, max_iter=10000, polish=True by default,
# however, we choose the OSQP default values here, as they are used for code generation as well
val = prob.solve(eps_abs=1e-3, eps_rel=1e-3, max_iter=4000, polish=False)
t1 = time.time()
print('\nCVXPY\nSolve time: %.3f ms' % (1000 * (t1 - t0)))
print('Objective function value: %.6f\n' % val)

# solve problem with C code via python wrapper
prob.register_solve('CPG', cpg_solve)
t0 = time.time()
val = prob.solve(method='CPG')
t1 = time.time()
print('\nCVXPYgen\nSolve time: %.3f ms' % (1000 * (t1 - t0)))
print('Objective function value: %.6f\n' % val)
```

\[1\] Wang, Yang, and Stephen Boyd. "Fast model predictive control using online optimization." IEEE Transactions on control systems technology 18.2 (2009): 267-278.

\[2\] Hovgaard, T. G., Larsen, L. F., Jørgensen, J. B., and Boyd, S.  "MPC for wind power gradients—utilizing forecasts, rotor inertia, and central energy storage." 2013 European Control Conference (ECC). IEEE, 2013.

a2a2921f05e8b982d31124fb62b1e2a19fe8d3b9
ipynb
Jupyter Notebook
examples/MPC.ipynb
cvxgrp/cvxpygen
5aaacdb894354288e2f12a16ca738248c69181c4
Apache-2.0
2022-02-25T03:30:06.000Z
2022-03-30T16:24:40.000Z
examples/MPC.ipynb
cvxgrp/cvxpygen
5aaacdb894354288e2f12a16ca738248c69181c4
Apache-2.0
examples/MPC.ipynb
cvxgrp/cvxpygen
5aaacdb894354288e2f12a16ca738248c69181c4
Apache-2.0
Qwen/Qwen-72B
1. YES
2. YES