# Advanced MPC Example

The following example is taken from a real application, the automatic racing of 1:43 RC race cars. The main idea is to formulate the problem “race fast while avoiding obstacles and staying on the track” as a sequential QP.

Further details on the project can be found on the ORCA project website.

## Introduction

We intend to solve the following MPC problem, which we term Model Predictive Contouring Control (MPCC) for the race cars:

Note that except the matrix , everything in this problem is a parameter, while we require the initial state to be zero. This stems from the fact that we re-linearize the nonlinear equations in the current operating point to yield a convex QP, particularly the car dynamics. Also note that we are penalizing the *deviation* from the previous control input. This is not directly supported by the multistage problem formulation, so we have to introduce lifting variables :

where is the input that has been applied to the system at the previous sampling instant. Note that we have upper and lower bounds on all variables to ensure unboundedness of the solution even in case of positive semidefinite , while we demand that .

## Problem transcription into FORCES format

Now let's begin transcribing the problem into the FORCES representation. Since everything depends on the way we define our stage variables, let us do this first.

### Stage variables

We define our stage variables as for and .

Now let us define the dimensions and cost matrices for the MPCC problem using these stage variables. We also get the `stages`

data structure:

N = 13; nx = 5; nu = 3; r = eye(nu); R = [r -r; -r r]; Q = 10*eye(nx); stages = MultistageProblem(N+1);

### Initial stage

From the definition of the stage variables, we can express the first stage as follows:

%% first stage i=1; istr = sprintf('%02d',i); % dimensions stages(i).dims.n = nx+2*nu; % number of stage variables stages(i).dims.r = 2*nx+2*nu; % number of equality constraints stages(i).dims.l = stages(i).dims.n; % number of lower bounds stages(i).dims.u = stages(i).dims.n; % 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); params(1) = newParam('f00', 1, 'cost.f'); % upper/lower bounds params(end+1) = newParam(['lb',istr], i, 'ineq.b.lb'); stages(i).ineq.b.lbidx = 1:stages(i).dims.n; params(end+1) = newParam(['ub',istr], i, 'ineq.b.ub'); stages(i).ineq.b.ubidx = 1:stages(i).dims.n;

Note that the linear term of the cost is a parameter. We still need to define the equality constraints. For the first stage, these are given by

Since and contain matrices that are varying, we define them as parameters, while is fixed and is defined directly in Matlab. Note that we have to define with stage 2!

% equality constraints params(end+1) = newParam(['C',istr], i, 'eq.C'); params(end+1) = newParam(['c',istr], i, 'eq.c'); stages(2).eq.D = [zeros(nx+nu,nx+2*nu); -eye(nx), zeros(nx,2*nu); zeros(nu,nx), -eye(nu), zeros(nu)];

### Stages along horizon

Similarly, the stages along the horizon with the difference that the cost is now a parameter and we additionally have affine inequalities:

%% stages along horizon for i = 2:N istr = sprintf('%02d',i); % dimensions stages(i).dims.n = nx+2*nu; % number of stage variables if( i < N ) stages(i).dims.r = nx+nu; % number of equality constraints else stages(i).dims.r = nx; % number of equality constraints end stages(i).dims.l = stages(i).dims.n; % number of lower bounds stages(i).dims.u = stages(i).dims.n; % number of upper bounds stages(i).dims.p = 2; % number of polytopic constraints stages(i).dims.q = 0; % number of quadratic constraints % cost params(end+1) = newParam(['H',istr], i, 'cost.H'); params(end+1) = newParam(['f',istr], i, 'cost.f'); % upper/lower bounds params(end+1) = newParam(['lb',istr], i, 'ineq.b.lb'); stages(i).ineq.b.lbidx = 1:stages(i).dims.n; params(end+1) = newParam(['ub',istr], i, 'ineq.b.ub'); stages(i).ineq.b.ubidx = 1:stages(i).dims.n; % affine inequality constraints params(end+1) = newParam(['A',istr], i, 'ineq.p.A'); params(end+1) = newParam(['b',istr], i, 'ineq.p.b'); % equality constraints params(end+1) = newParam(['C',istr], i, 'eq.C'); params(end+1) = newParam(['c',istr], i, 'eq.c'); if( i < N ) stages(i+1).eq.D = [-eye(nx), zeros(nx,2*nu); zeros(nu,nx), -eye(nu), zeros(nu)]; else stages(i+1).eq.D = -eye(nx); end end

### Final stage

The final stage is different in that it does not have and since :

%% final stage i = N+1; istr = sprintf('%02d',i); % dimensions stages(i).dims.n = nx; % number of stage variables stages(i).dims.r = 0; % number of equality constraints stages(i).dims.l = stages(i).dims.n; % number of lower bounds stages(i).dims.u = stages(i).dims.n; % number of upper bounds stages(i).dims.p = 2; % number of polytopic constraints stages(i).dims.q = 0; % number of quadratic constraints % cost params(end+1) = newParam('P', i, 'cost.H'); stages(i).cost.f = zeros(nx,1); % upper/lower bounds params(end+1) = newParam(['lb',istr], i, 'ineq.b.lb'); stages(i).ineq.b.lbidx = 1:stages(i).dims.n; params(end+1) = newParam(['ub',istr], i, 'ineq.b.ub'); stages(i).ineq.b.ubidx = 1:stages(i).dims.n; % affine inequality constraints params(end+1) = newParam(['A',istr], i, 'ineq.p.A'); params(end+1) = newParam(['b',istr], i, 'ineq.p.b');