# Model predictive control (MPC)¶

We consider the problem of controlling a linear time-invariant dynamical system to some reference state $$x_r \in \mathbf{R}^{n_x}$$. To achieve this we use constrained linear-quadratic MPC, which solves at each time step the following finite-horizon optimal control problem

$\begin{split}\begin{array}{ll} \mbox{minimize} & (x_N-x_r)^T Q_N (x_N-x_r) + \sum_{k=0}^{N-1} (x_k-x_r)^T Q (x_k-x_r) + u_k^T R u_k \\ \mbox{subject to} & x_{k+1} = A x_k + B u_k \\ & x_{\rm min} \le x_k \le x_{\rm max} \\ & u_{\rm min} \le u_k \le u_{\rm max} \\ & x_0 = \bar{x} \end{array}\end{split}$

The states $$x_k \in \mathbf{R}^{n_x}$$ and the inputs $$u_k \in \mathbf{R}^{n_u}$$ are constrained to be between some lower and upper bounds. The problem is solved repeatedly for varying initial state $$\bar{x} \in \mathbf{R}^{n_x}$$.

## Python¶

import osqp
import numpy as np
import scipy as sp
from scipy import sparse

# Discrete time model of a quadcopter
[1.,      0.,     0., 0., 0., 0., 0.1,     0.,     0.,  0.,     0.,     0.    ],
[0.,      1.,     0., 0., 0., 0., 0.,      0.1,    0.,  0.,     0.,     0.    ],
[0.,      0.,     1., 0., 0., 0., 0.,      0.,     0.1, 0.,     0.,     0.    ],
[0.0488,  0.,     0., 1., 0., 0., 0.0016,  0.,     0.,  0.0992, 0.,     0.    ],
[0.,     -0.0488, 0., 0., 1., 0., 0.,     -0.0016, 0.,  0.,     0.0992, 0.    ],
[0.,      0.,     0., 0., 0., 1., 0.,      0.,     0.,  0.,     0.,     0.0992],
[0.,      0.,     0., 0., 0., 0., 1.,      0.,     0.,  0.,     0.,     0.    ],
[0.,      0.,     0., 0., 0., 0., 0.,      1.,     0.,  0.,     0.,     0.    ],
[0.,      0.,     0., 0., 0., 0., 0.,      0.,     1.,  0.,     0.,     0.    ],
[0.9734,  0.,     0., 0., 0., 0., 0.0488,  0.,     0.,  0.9846, 0.,     0.    ],
[0.,     -0.9734, 0., 0., 0., 0., 0.,     -0.0488, 0.,  0.,     0.9846, 0.    ],
[0.,      0.,     0., 0., 0., 0., 0.,      0.,     0.,  0.,     0.,     0.9846]
])
Bd = sparse.csc_matrix([
[0.,      -0.0726,  0.,     0.0726],
[-0.0726,  0.,      0.0726, 0.    ],
[-0.0152,  0.0152, -0.0152, 0.0152],
[-0.,     -0.0006, -0.,     0.0006],
[0.0006,   0.,     -0.0006, 0.0000],
[0.0106,   0.0106,  0.0106, 0.0106],
[0,       -1.4512,  0.,     1.4512],
[-1.4512,  0.,      1.4512, 0.    ],
[-0.3049,  0.3049, -0.3049, 0.3049],
[-0.,     -0.0236,  0.,     0.0236],
[0.0236,   0.,     -0.0236, 0.    ],
[0.2107,   0.2107,  0.2107, 0.2107]])
[nx, nu] = Bd.shape

# Constraints
u0 = 10.5916
umin = np.array([9.6, 9.6, 9.6, 9.6]) - u0
umax = np.array([13., 13., 13., 13.]) - u0
xmin = np.array([-np.pi/6,-np.pi/6,-np.inf,-np.inf,-np.inf,-1.,
-np.inf,-np.inf,-np.inf,-np.inf,-np.inf,-np.inf])
xmax = np.array([ np.pi/6, np.pi/6, np.inf, np.inf, np.inf, np.inf,
np.inf, np.inf, np.inf, np.inf, np.inf, np.inf])

# Objective function
Q = sparse.diags([0., 0., 10., 10., 10., 10., 0., 0., 0., 5., 5., 5.])
QN = Q
R = 0.1*sparse.eye(4)

# Initial and reference states
x0 = np.zeros(12)
xr = np.array([0.,0.,1.,0.,0.,0.,0.,0.,0.,0.,0.,0.])

# Prediction horizon
N = 10

# Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
P = sparse.block_diag([sparse.kron(sparse.eye(N), Q), QN,
sparse.kron(sparse.eye(N), R)], format='csc')
# - linear objective
q = np.hstack([np.kron(np.ones(N), -Q.dot(xr)), -QN.dot(xr),
np.zeros(N*nu)])
# - linear dynamics
Ax = sparse.kron(sparse.eye(N+1),-sparse.eye(nx)) + sparse.kron(sparse.eye(N+1, k=-1), Ad)
Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), Bd)
Aeq = sparse.hstack([Ax, Bu])
leq = np.hstack([-x0, np.zeros(N*nx)])
ueq = leq
# - input and state constraints
Aineq = sparse.eye((N+1)*nx + N*nu)
lineq = np.hstack([np.kron(np.ones(N+1), xmin), np.kron(np.ones(N), umin)])
uineq = np.hstack([np.kron(np.ones(N+1), xmax), np.kron(np.ones(N), umax)])
# - OSQP constraints
A = sparse.vstack([Aeq, Aineq], format='csc')
l = np.hstack([leq, lineq])
u = np.hstack([ueq, uineq])

# Create an OSQP object
prob = osqp.OSQP()

# Setup workspace
prob.setup(P, q, A, l, u, warm_start=True)

# Simulate in closed loop
nsim = 15
for i in range(nsim):
# Solve
res = prob.solve()

# Check solver status
if res.info.status != 'solved':
raise ValueError('OSQP did not solve the problem!')

# Apply first control input to the plant
ctrl = res.x[-N*nu:-(N-1)*nu]

# Update initial state
l[:nx] = -x0
u[:nx] = -x0
prob.update(l=l, u=u)


## Matlab¶

% Discrete time model of a quadcopter
Ad = [1       0       0   0   0   0   0.1     0       0    0       0       0;
0       1       0   0   0   0   0       0.1     0    0       0       0;
0       0       1   0   0   0   0       0       0.1  0       0       0;
0.0488  0       0   1   0   0   0.0016  0       0    0.0992  0       0;
0      -0.0488  0   0   1   0   0      -0.0016  0    0       0.0992  0;
0       0       0   0   0   1   0       0       0    0       0       0.0992;
0       0       0   0   0   0   1       0       0    0       0       0;
0       0       0   0   0   0   0       1       0    0       0       0;
0       0       0   0   0   0   0       0       1    0       0       0;
0.9734  0       0   0   0   0   0.0488  0       0    0.9846  0       0;
0      -0.9734  0   0   0   0   0      -0.0488  0    0       0.9846  0;
0       0       0   0   0   0   0       0       0    0       0       0.9846];
Bd = [0      -0.0726  0       0.0726;
-0.0726  0       0.0726  0;
-0.0152  0.0152 -0.0152  0.0152;
0      -0.0006 -0.0000  0.0006;
0.0006  0      -0.0006  0;
0.0106  0.0106  0.0106  0.0106;
0      -1.4512  0       1.4512;
-1.4512  0       1.4512  0;
-0.3049  0.3049 -0.3049  0.3049;
0      -0.0236  0       0.0236;
0.0236  0      -0.0236  0;
0.2107  0.2107  0.2107  0.2107];
[nx, nu] = size(Bd);

% Constraints
u0 = 10.5916;
umin = [9.6; 9.6; 9.6; 9.6] - u0;
umax = [13; 13; 13; 13] - u0;
xmin = [-pi/6; -pi/6; -Inf; -Inf; -Inf; -1; -Inf(6,1)];
xmax = [ pi/6;  pi/6;  Inf;  Inf;  Inf; Inf; Inf(6,1)];

% Objective function
Q = diag([0 0 10 10 10 10 0 0 0 5 5 5]);
QN = Q;
R = 0.1*eye(4);

% Initial and reference states
x0 = zeros(12,1);
xr = [0; 0; 1; 0; 0; 0; 0; 0; 0; 0; 0; 0];

% Prediction horizon
N = 10;

% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; zeros(N*nu, 1)];
% - linear dynamics
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), Ad);
Bu = kron([sparse(1, N); speye(N)], Bd);
Aeq = [Ax, Bu];
leq = [-x0; zeros(N*nx, 1)];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq];
l = [leq; lineq];
u = [ueq; uineq];

% Create an OSQP object
prob = osqp;

% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);

% Simulate in closed loop
nsim = 15;
for i = 1 : nsim
% Solve
res = prob.solve();

% Check solver status
if ~strcmp(res.info.status, 'solved')
error('OSQP did not solve the problem!')
end

% Apply first control input to the plant
ctrl = res.x((N+1)*nx+1:(N+1)*nx+nu);

% Update initial state
l(1:nx) = -x0;
u(1:nx) = -x0;
prob.update('l', l, 'u', u);
end


## CVXPY¶

from cvxpy import *
import numpy as np
import scipy as sp
from scipy import sparse

# Discrete time model of a quadcopter
[1.,      0.,     0., 0., 0., 0., 0.1,     0.,     0.,  0.,     0.,     0.    ],
[0.,      1.,     0., 0., 0., 0., 0.,      0.1,    0.,  0.,     0.,     0.    ],
[0.,      0.,     1., 0., 0., 0., 0.,      0.,     0.1, 0.,     0.,     0.    ],
[0.0488,  0.,     0., 1., 0., 0., 0.0016,  0.,     0.,  0.0992, 0.,     0.    ],
[0.,     -0.0488, 0., 0., 1., 0., 0.,     -0.0016, 0.,  0.,     0.0992, 0.    ],
[0.,      0.,     0., 0., 0., 1., 0.,      0.,     0.,  0.,     0.,     0.0992],
[0.,      0.,     0., 0., 0., 0., 1.,      0.,     0.,  0.,     0.,     0.    ],
[0.,      0.,     0., 0., 0., 0., 0.,      1.,     0.,  0.,     0.,     0.    ],
[0.,      0.,     0., 0., 0., 0., 0.,      0.,     1.,  0.,     0.,     0.    ],
[0.9734,  0.,     0., 0., 0., 0., 0.0488,  0.,     0.,  0.9846, 0.,     0.    ],
[0.,     -0.9734, 0., 0., 0., 0., 0.,     -0.0488, 0.,  0.,     0.9846, 0.    ],
[0.,      0.,     0., 0., 0., 0., 0.,      0.,     0.,  0.,     0.,     0.9846]
])
Bd = sparse.csc_matrix([
[0.,      -0.0726,  0.,     0.0726],
[-0.0726,  0.,      0.0726, 0.    ],
[-0.0152,  0.0152, -0.0152, 0.0152],
[-0.,     -0.0006, -0.,     0.0006],
[0.0006,   0.,     -0.0006, 0.0000],
[0.0106,   0.0106,  0.0106, 0.0106],
[0,       -1.4512,  0.,     1.4512],
[-1.4512,  0.,      1.4512, 0.    ],
[-0.3049,  0.3049, -0.3049, 0.3049],
[-0.,     -0.0236,  0.,     0.0236],
[0.0236,   0.,     -0.0236, 0.    ],
[0.2107,   0.2107,  0.2107, 0.2107]])
[nx, nu] = Bd.shape

# Constraints
u0 = 10.5916
umin = np.array([9.6, 9.6, 9.6, 9.6]) - u0
umax = np.array([13., 13., 13., 13.]) - u0
xmin = np.array([-np.pi/6,-np.pi/6,-np.inf,-np.inf,-np.inf,-1.,
-np.inf,-np.inf,-np.inf,-np.inf,-np.inf,-np.inf])
xmax = np.array([ np.pi/6, np.pi/6, np.inf, np.inf, np.inf, np.inf,
np.inf, np.inf, np.inf, np.inf, np.inf, np.inf])

# Objective function
Q = sparse.diags([0., 0., 10., 10., 10., 10., 0., 0., 0., 5., 5., 5.])
QN = Q
R = 0.1*sparse.eye(4)

# Initial and reference states
x0 = np.zeros(12)
xr = np.array([0.,0.,1.,0.,0.,0.,0.,0.,0.,0.,0.,0.])

# Prediction horizon
N = 10

# Define problem
u = Variable((nu, N))
x = Variable((nx, N+1))
x_init = Parameter(nx)
objective = 0
constraints = [x[:,0] == x_init]
for k in range(N):
constraints += [x[:,k+1] == Ad*x[:,k] + Bd*u[:,k]]
constraints += [xmin <= x[:,k], x[:,k] <= xmax]
constraints += [umin <= u[:,k], u[:,k] <= umax]
objective += quad_form(x[:,N] - xr, QN)
prob = Problem(Minimize(objective), constraints)

# Simulate in closed loop
nsim = 15
for i in range(nsim):
x_init.value = x0
prob.solve(solver=OSQP, warm_start=True)


## YALMIP¶

% Discrete time model of a quadcopter
Ad = [1       0       0   0   0   0   0.1     0       0    0       0       0;
0       1       0   0   0   0   0       0.1     0    0       0       0;
0       0       1   0   0   0   0       0       0.1  0       0       0;
0.0488  0       0   1   0   0   0.0016  0       0    0.0992  0       0;
0      -0.0488  0   0   1   0   0      -0.0016  0    0       0.0992  0;
0       0       0   0   0   1   0       0       0    0       0       0.0992;
0       0       0   0   0   0   1       0       0    0       0       0;
0       0       0   0   0   0   0       1       0    0       0       0;
0       0       0   0   0   0   0       0       1    0       0       0;
0.9734  0       0   0   0   0   0.0488  0       0    0.9846  0       0;
0      -0.9734  0   0   0   0   0      -0.0488  0    0       0.9846  0;
0       0       0   0   0   0   0       0       0    0       0       0.9846];
Bd = [0      -0.0726  0       0.0726;
-0.0726  0       0.0726  0;
-0.0152  0.0152 -0.0152  0.0152;
0      -0.0006 -0.0000  0.0006;
0.0006  0      -0.0006  0;
0.0106  0.0106  0.0106  0.0106;
0      -1.4512  0       1.4512;
-1.4512  0       1.4512  0;
-0.3049  0.3049 -0.3049  0.3049;
0      -0.0236  0       0.0236;
0.0236  0      -0.0236  0;
0.2107  0.2107  0.2107  0.2107];
[nx, nu] = size(Bd);

% Constraints
u0 = 10.5916;
umin = [9.6; 9.6; 9.6; 9.6] - u0;
umax = [13; 13; 13; 13] - u0;
xmin = [-pi/6; -pi/6; -Inf; -Inf; -Inf; -1; -Inf(6,1)];
xmax = [ pi/6;  pi/6;  Inf;  Inf;  Inf; Inf; Inf(6,1)];

% Objective function
Q = diag([0 0 10 10 10 10 0 0 0 5 5 5]);
QN = Q;
R = 0.1*eye(4);

% Initial and reference states
x0 = zeros(12,1);
xr = [0; 0; 1; 0; 0; 0; 0; 0; 0; 0; 0; 0];

% Prediction horizon
N = 10;

% Define problem
u = sdpvar(repmat(nu,1,N), repmat(1,1,N));
x = sdpvar(repmat(nx,1,N+1), repmat(1,1,N+1));
constraints = [xmin <= x{1} <= xmax];
objective = 0;
for k = 1 : N
objective = objective + (x{k}-xr)'*Q*(x{k}-xr) + u{k}'*R*u{k};
constraints = [constraints, x{k+1} == Ad*x{k} + Bd*u{k}];
constraints = [constraints, umin <= u{k}<= umax, xmin <= x{k+1} <= xmax];
end
objective = objective + (x{N+1}-xr)'*QN*(x{N+1}-xr);
options = sdpsettings('solver', 'osqp');
controller = optimizer(constraints, objective, options, x{1}, [u{:}]);

% Simulate in closed loop
nsim = 15;
for i = 1 : nsim
U = controller{x0};
end


## Julia¶

# Add packages - uncomment for first-time setup

using SparseArrays, OSQP

# Utility function
speye(N) = spdiagm(ones(N))

# Discrete time model of a quadcopter
Ad = [1       0       0   0   0   0   0.1     0       0    0       0       0;
0       1       0   0   0   0   0       0.1     0    0       0       0;
0       0       1   0   0   0   0       0       0.1  0       0       0;
0.0488  0       0   1   0   0   0.0016  0       0    0.0992  0       0;
0      -0.0488  0   0   1   0   0      -0.0016  0    0       0.0992  0;
0       0       0   0   0   1   0       0       0    0       0       0.0992;
0       0       0   0   0   0   1       0       0    0       0       0;
0       0       0   0   0   0   0       1       0    0       0       0;
0       0       0   0   0   0   0       0       1    0       0       0;
0.9734  0       0   0   0   0   0.0488  0       0    0.9846  0       0;
0      -0.9734  0   0   0   0   0      -0.0488  0    0       0.9846  0;
0       0       0   0   0   0   0       0       0    0       0       0.9846] |> sparse
Bd = [0      -0.0726  0       0.0726;
-0.0726  0       0.0726  0;
-0.0152  0.0152 -0.0152  0.0152;
0      -0.0006 -0.0000  0.0006;
0.0006  0      -0.0006  0;
0.0106  0.0106  0.0106  0.0106;
0      -1.4512  0       1.4512;
-1.4512  0       1.4512  0;
-0.3049  0.3049 -0.3049  0.3049;
0      -0.0236  0       0.0236;
0.0236  0      -0.0236  0;
0.2107  0.2107  0.2107  0.2107] |> sparse
(nx, nu) = size(Bd)

# Constraints
u0 = 10.5916
umin = [9.6, 9.6, 9.6, 9.6] .- u0
umax = [13, 13, 13, 13] .- u0
xmin = [[-pi/6, -pi/6, -Inf, -Inf, -Inf, -1]; -Inf .* ones(6)]
xmax = [[pi/6,  pi/6,  Inf,  Inf,  Inf, Inf]; Inf .* ones(6)]

# Objective function
Q = spdiagm([0, 0, 10, 10, 10, 10, 0, 0, 0, 5, 5, 5])
QN = Q
R = 0.1 * speye(nu)

# Initial and reference states
x0 = zeros(12)
xr = [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0]

# Prediction horizon
N = 10

# Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
P = blockdiag(kron(speye(N), Q), QN, kron(speye(N), R))
# - linear objective
q = [repeat(-Q * xr, N); -QN * xr; zeros(N*nu)]
# - linear dynamics
Ax = kron(speye(N + 1), -speye(nx)) + kron(spdiagm(-1 => ones(N)), Ad)
Bu = kron([spzeros(1, N); speye(N)], Bd)
Aeq = [Ax Bu]
leq = [-x0; zeros(N * nx)]
ueq = leq
# - input and state constraints
Aineq = speye((N + 1) * nx + N * nu)
lineq = [repeat(xmin, N + 1); repeat(umin, N)]
uineq = [repeat(xmax, N + 1); repeat(umax, N)]
# - OSQP constraints
A, l, u = [Aeq; Aineq], [leq; lineq], [ueq; uineq]

# Create an OSQP model
m = OSQP.Model()

# Setup workspace
OSQP.setup!(m; P=P, q=q, A=A, l=l, u=u, warm_start=true)

# Simulate in closed loop
nsim = 15;
@time for _ in 1 : nsim
# Solve
res = OSQP.solve!(m)

# Check solver status
if res.info.status != :Solved
error("OSQP did not solve the problem!")
end

# Apply first control input to the plant
ctrl = res.x[(N+1)*nx+1:(N+1)*nx+nu]
global x0 = Ad * x0 + Bd * ctrl

# Update initial state
l[1:nx], u[1:nx] = -x0, -x0
OSQP.update!(m; l=l, u=u)
end