Simple MPC example

Introduction

This example shows how to generate code for a simple MPC problem for regulation. You can download the complete code here.

\begin{equation*}
\begin{aligned}
\min_{x_i,u_i}\ &x_N^T P x_N + \sum_{i=1}^{N-1} x_i^T Q x_i + u_i^T R u_i\\
\text{s.t.}\ & x_1 = x\enspace, \\
& x_{i+1} = Ax_i + B u_i\enspace,\quad i=1,\dots,N-1\enspace,\\
& x_{\text{min}} \leq x_i \leq x_{\text{max}}\enspace,\quad\; i=1,\dots,N\enspace, \\
& u_{\text{min}} \leq u_i \leq u_{\text{max}}\enspace,\quad\; i=1,\dots,N\enspace.
\end{aligned}
\end{equation*}

Note that we denote the current state with $x_1$ for convenience.

MPC problem

Let us first define some system - in this case an unstable double integrator with system matrices
$A = \begin{bmatrix} 1.1 & 1 \\ 0 & 1 \end{bmatrix}$ and $B = \begin{bmatrix} 1 \\ 0.5 \end{bmatrix}\;\;\;$:

%% system
nx = 2;
nu = 1;
A = [1.1 1; 0 1];
B = [1; 0.5];

Next, we define the data needed for the MPC problem. The constraints on the states are $x_{\textrm{max}} = -x_{\textrm{min}} = 5$ and on the inputs $u_{\textrm{max}} = -u_{\textrm{min}} = 0.5$. We choose $Q = I_2$ and $R = 1$, while the terminal cost is given by the solution to the Ricatti equation related to the LQR problem:

%% MPC setup
N = 10;
Q = eye(nx);
R = eye(nu);
[~,P] = dlqr(A,B,Q,R);
umin = -0.5;     umax = 0.5;
xmin = [-5; -5]; xmax = [5; 5];

Formulating the problem in FORCES syntax

In the following, we are going to define the MPC problem in FORCES syntax. First, you have to create a stages struct of appropriate length:

%% FORCES multistage form - zi = [xi, ui] for i=1...N-1 and zN = xN
stages = MultistageProblem(N);

This struct is filled with the numerical problem data $H_i, f_i, C_i, D_i, c_i, \underline{z}_i, \bar{z}_i$ (if they are not a parameter of the problem). The ordering of the variables is up to you, we choose $z_i = [x_i\ u_i]\ \forall i<N$ and $z_N = x_N$. Hence, stage 1 is defined as follows:

%% stage 1
i = 1
 
% dimension
stages(i).dims.n = nx+nu; % number of stage variables
stages(i).dims.r = 2*nx;  % number of equality constraints        
stages(i).dims.l = nx+nu; % number of lower bounds
stages(i).dims.u = nx+nu; % number of upper bounds
stages(i).dims.p = 0;     % number of polytopic constraints
stages(i).dims.q = 0;     % number of quadratic constraints
 
% cost
stages(i).cost.H = blkdiag(Q,R);
stages(i).cost.f = zeros(stages(i).dims.n,1);
 
% lower bounds
stages(i).ineq.b.lbidx = 1:stages(i).dims.n; % lower bound acts on these indices
stages(i).ineq.b.lb = [xmin; umin]; % lower bound for this stage variable
 
% upper bounds
stages(i).ineq.b.ubidx = 1:stages(i).dims.n; % upper bound acts on these indices
stages(i).ineq.b.ub = [xmax; umax]; % upper bound for this stage variable
 
% equality constraints
stages(i).eq.C = [eye(nx), zeros(nx,nu); A, B];
params(1) = newParam('z1',1,'eq.c'); % RHS of first eq. constr. is a parameter: [x0, 0]

In the last line we define $c_1$ of the first equality constraint $C_1 z_1 + D_2 z_2 = c_1$ as a parameter, since this one contains the initial condition $x_1 = x$, which is a parameter to our solver.

The stages along the horizon are defined similarly (note that $D_2$ has to be defined a bit differently due to the initial condition equality constraint):

%% stages along the horizon
for i = 2:N-1
    % dimension
    stages(i).dims.n = nx+nu; % number of stage variables
    stages(i).dims.r = nx;    % number of equality constraints        
    stages(i).dims.l = nx+nu; % number of lower bounds
    stages(i).dims.u = nx+nu; % number of upper bounds
    stages(i).dims.p = 0;     % number of polytopic constraints
    stages(i).dims.q = 0;     % number of quadratic constraints
 
    % cost
    stages(i).cost.H = blkdiag(Q,R);
    stages(i).cost.f = zeros(stages(i).dims.n,1);
 
    % lower bounds
    stages(i).ineq.b.lbidx = 1:stages(i).dims.n; % lower bound acts on these indices
    stages(i).ineq.b.lb = [xmin; umin]; % lower bound for this stage variable
 
    % upper bounds
    stages(i).ineq.b.ubidx = 1:stages(i).dims.n; % upper bound acts on these indices
    stages(i).ineq.b.ub = [xmax; umax]; % upper bound for this stage variable
 
    % equality constraints
    stages(i).eq.C = [A, B];
    stages(i).eq.c = zeros(nx,1);
    if( i==2 )
        stages(i).eq.D = [zeros(nx,nx+nu); -eye(nx), zeros(nx,nu)];
    else
        stages(i).eq.D = [-eye(nx), zeros(nx,nu)];
    end
end

Similarly for the last stage:

% final stage
i = N;
 
% dimension
stages(i).dims.n = nx;    % number of stage variables
stages(i).dims.r = 0;     % number of equality constraints        
stages(i).dims.l = nx;    % number of lower bounds
stages(i).dims.u = nx;    % number of upper bounds
stages(i).dims.p = 0;     % number of polytopic constraints
stages(i).dims.q = 0;     % number of quadratic constraints
 
% cost
stages(i).cost.H = P;
stages(i).cost.f = zeros(stages(i).dims.n,1);
 
% lower bounds
stages(i).ineq.b.lbidx = 1:stages(i).dims.n; % lower bound acts on these indices
stages(i).ineq.b.lb = xmin; % lower bound for this stage variable
 
% upper bounds
stages(i).ineq.b.ubidx = 1:stages(i).dims.n; % upper bound acts on these indices
stages(i).ineq.b.ub = xmax; % upper bound for this stage variable
 
% equality constraints        
stages(i).eq.D = -eye(nx);

Defining outputs

You can specify which variables should be an output of the solver. For example, for closed-loop simulations with your MPC controller, only the first control input $u_1$ is needed. You can use the function newOutput of the MATLAB client to define outputs of the solver as an array of structs (see help newOutput for details):

%% define outputs of the solver
outputs = newOutput('u1', 1, nx+1:nx+nu);

The first argument is the name of the output, the second tells FORCES from which stage variable index the output shall be copied out, and the last argument is an array with indices within the selected stage variable that correspond to the output.

Options

You can pass an options struct to the code generator containing some settings for the solver. In this example, we set the name of the solver by using

%% solver settings
codeoptions = getOptions('myMPC');

You can set parameters of the line search etc., too - see help getOptions for more details.

Generating the solver

Once you have defined your problem, you can generate code using

generateCode(stages, params, codeoptions, outputs);

This will send your problem data to the server and retrieve the generated code, unpack it and compile it. Once the code generation is finished, you will have a solver myMPC (or any other name you have chosen) ready to use. Type

help myMPC

to see how to call the solver from MATLAB. The C source files reside in the directory myMPC.

Closed-loop simulation

We are now ready to use the solver in closed-loop simulations:

%% simulate
x1 = [-4; 2];
kmax = 30;
X = zeros(nx,kmax+1); X(:,1) = x1;
U = zeros(nu,kmax);
problem.z1 = zeros(2*nx,1);
for k = 1:kmax
    problem.z1(1:nx) = X(:,k);
    [solverout,exitflag,info] = myMPC(problem);
    if( exitflag == 1 )
        U(:,k) = solverout.u1;
    else
        info
        error('Some problem in solver');
    end
    X(:,k+1) = A*X(:,k) + B*U(:,k);
end

Note that the initial state $x_1$ should be feasible for your problem - the solver does not detect infeasibility.

Finally, let us plot the result:

%% plot
figure(1); clf;
subplot(2,1,1); grid on; title('states'); hold on;
plot([1 kmax], [xmax xmax]', 'r--'); plot([1 kmax], [xmin xmin]', 'r--');
ylim(1.1*[min(xmin),max(xmax)]); stairs(1:kmax,X(:,1:kmax)');
subplot(2,1,2);  grid on; title('input'); hold on;
plot([1 kmax], [umax umax]', 'r--'); plot([1 kmax], [umin umin]', 'r--');
ylim(1.1*[min(umin),max(umax)]); stairs(1:kmax,U(:,1:kmax)');