# Simulate and Generate Code for MPC Controller with Custom QP Solver

This example shows how to simulate and generate code for a model predictive controller that uses a custom quadratic programming (QP) solver. The plant for this example is a dc-servo motor in Simulink®.

## Contents

## DC-Servo Motor Model

The dc-servo motor model is a linear dynamic system described in [1]. `plant` is the continuous-time state-space model of the motor. `tau` is the maximum admissible torque, which you use as an output constraint.

[plant,tau] = mpcmotormodel;

## Design MPC Controller

The plant has one input, the motor input voltage. The MPC controller uses this input as a manipulated variable (`MV`). The plant has two outputs, the motor angular position and shaft torque. The angular position is a measured output (`MO`), and the shaft torque is unmeasured (`UO`).

plant = setmpcsignals(plant,'MV',1,'MO',1,'UO',2);

Constrain the manipulated variable to be between +/- `220` volts. Since the plant inputs and outputs are of different orders of magnitude, to facilitate tuning, use scale factors. Typical choices of scale factor are the upper/lower limit or the operating range.

MV = struct('Min',-220,'Max',220,'ScaleFactor',440);

There is no constraint on the angular position. Specify upper and lower bounds on shaft torque during the first three prediction horizon steps. To define these bounds, use `tau`.

OV = struct('Min',{-Inf, [-tau;-tau;-tau;-Inf]},... 'Max',{Inf, [tau;tau;tau;Inf]},'ScaleFactor',{2*pi, 2*tau});

The control task is to achieve zero tracking error for the angular position. Since you only have one manipulated variable, allow shaft torque to float within its constraint by setting its tuning weight to zero.

Weights = struct('MV',0,'MVRate',0.1,'OV',[0.1 0]);

Specify the sample time and horizons, and create the MPC controller, using `plant` as the predictive model.

Ts = 0.1; % Sample time p = 10; % Prediction horizon m = 2; % Control horizon mpcobj = mpc(plant,Ts,p,m,Weights,MV,OV);

## Simulate in Simulink with Built-In QP Solver

To run the remaining example, Simulink is required.

if ~mpcchecktoolboxinstalled('simulink') disp('Simulink is required to run this example.') return end

Open a Simulink model that simulates closed-loop control of the dc-servo motor using the MPC controller. By default, MPC uses a built-in QP solver that uses the KWIK algorithm.

```
mdl = 'mpc_customQPcodegen';
open_system(mdl)
```

Run the simulation

sim(mdl)

-->Converting model to discrete time. Assuming no disturbance added to measured output channel #1. -->The "Model.Noise" property of the "mpc" object is empty. Assuming white noise on each measured output channel.

Store the plant input and output signals in the MATLAB workspace.

uKWIK = u; yKWIK = y;

## Simulate in Simulink with a Custom QP Solver

To examine how the custom solver behaves under the same conditions, enable the custom solver in the MPC controller.

mpcobj.Optimizer.CustomSolver = true;

You must also provide a MATLAB® function that satisfies the following requirements:

- Function name must be
`mpcCustomSolver`. - Input and output arguments must match the arguments in the template file.
- Function must be on the MATLAB path.

In this example, use the custom QP solver defined in the template file `mpcCustomSolverCodeGen_TemplateEML.txt`, which implements the `dantzig` algorithm and is suitable for code generation. Save the function in your working folder as `mpcCustomSolver.m`.

src = which('mpcCustomSolverCodeGen_TemplateEML.txt'); dest = fullfile(pwd,'mpcCustomSolver.m'); copyfile(src,dest,'f')

Simulate closed-loop control of the dc-servo motor, and save the plant input and output.

sim(mdl) uDantzigSim = u; yDantzigSim = y;

-->Converting model to discrete time. Assuming no disturbance added to measured output channel #1. -->The "Model.Noise" property of the "mpc" object is empty. Assuming white noise on each measured output channel.

## Generate Code with Custom QP Solver

To run the remaining example, Simulink Coder product is required.

if ~mpcchecktoolboxinstalled('simulinkcoder') disp('Simulink(R) Coder(TM) is required to run this example.') return end

To generate code from an MPC Controller block that uses a custom QP solver, enable the custom solver for code generation option in the MPC controller.

mpcobj.Optimizer.CustomSolverCodeGen = true;

You must also provide a MATLAB® function that satisfies all the following requirements:

- Function name must be
`mpcCustomSolverCodeGen`. - Input and output arguments must match the arguments in the template file.
- Function must be on the MATLAB path.

In this example, use the same custom solver defined in `mpcCustomSolverCodeGen_TemplateEML.txt`. Save the function in your working folder as `mpcCustomSolverCodeGen.m`.

src = which('mpcCustomSolverCodeGen_TemplateEML.txt'); dest = fullfile(pwd,'mpcCustomSolverCodeGen.m'); copyfile(src,dest,'f')

Review the saved `mpcCustomSolverCodeGen.m` file.

function [x, status] = mpcCustomSolverCodeGen(H, f, A, b, x0) %#codegen % mpcCustomSolverCodeGen allows the user to specify a custom (QP) solver % written in Embedded MATLAB to be used by MPC controller in code generation. % % Workflow: % (1) Copy this template file to your work folder and rename it to % "mpcCustomSolverCodeGen.m". The work folder must be on the path. % (2) Modify the "mpcCustomSolverCodeGen.m" to use your solver. % Note that your Embedded MATLAB solver must use only fixed-size data. % (3) Set "mpcobj.Optimizer.CustomSolverCodeGen = true" to tell the MPC % controller to use the solver in code generation. % To generate code: % In MATLAB, use "codegen" command with "mpcmoveCodeGeneration" (require MATLAB Coder) % In Simulink, generate code with MPC and Adaptive MPC blocks (require Simuink coder products) % % To use this solver for simulation in MATLAB and Simulink, you need to: % (1) Copy "mpcCustomSolver.txt" template file to your work folder and % rename it to "mpcCustomSolver.m". The work folder must be on the path. % (2) Modify the "mpcCustomSolver.m" to use your solver. % (3) Set "mpcobj.Optimizer.CustomSolver = true" to tell the MPC % controller to use the solver in simulation. % % The MPC QP problem is defined as follows: % % min J(x) = 0.5*x'*H*x + f'*x, s.t. A*x >= b. % % Inputs (provided by MPC controller at run-time): % H: a n-by-n Hessian matrix, which is symmetric and positive definite. % f: a n-by-1 column vector. % A: a m-by-n matrix of inequality constraint coefficients. % b: a m-by-1 vector of the right-hand side of inequality constraints. % x0: a n-by-1 vector of the initial guess of the optimal solution. % % Outputs (sent back to MPC controller at run-time): % x: must be a n-by-1 vector of optimal solution. % status: must be an integer of: % positive value: number of iterations used in computation % 0: maximum number of iterations reached % -1: QP is infeasible % -2: Failed to find a solution due to other reasons % Note that: % (1) When solver fails to find an optimal solution (status<=0), "x" % still needs to be returned. % (2) To use sub-optimal QP solution in MPC, return the sub-optimal "x" % with "status = 0". In addition, you need to set % "mpcobj.Optimizer.UseSuboptimalSolution = true" in MPC controller. % % DO NOT CHANGE LINES ABOVE % This template implements a showcase QP solver using "Dantzig" algorithm % by G. B. Dantzig, A. Orden, and P. Wolfe, "The generalized simplex method % for minimizing a linear form under linear inequality constraints", % Pacific J. of Mathematics, 5:183–195, 1955. % % User is expected to modify this template and plug in own custom QP solver % that replaces the "Dantzig" algorithm. ZERO = zeros('like',H); ONE = ones('like',H); % xmin is a constant term that adds to the initial basis because "dantzig" % requires positive optimization variables. A fixed "xmin" does not work % for all MPC problems. xmin = -1e3*ones(size(f(:)))*ONE; maxiter = 200*ONE; nvar = length(f); ncon = length(b); a = -H*xmin(:); H = H\eye(nvar); rhsc = A*xmin(:) - b(:); rhsa = a-f(:); TAB = -[H H*A';A*H A*H*A']; basisi = [H*rhsa; rhsc + A*H*rhsa]; ibi = -(1:nvar+ncon)'*ONE; ili = -ibi*ONE; %% Call EML function "qpdantzg" [basis,ib,il,iter] = qpdantzg(TAB,basisi,ibi,ili,maxiter); %#ok<ASGLU> %% status if iter > maxiter status = ZERO; elseif iter < ZERO status = -ONE; else status = iter; end %% optimal variable x = zeros(nvar,1,'like',H); for j = 1:nvar if il(j) <= ZERO x(j) = xmin(j); else x(j) = basis(il(j))+xmin(j); end end

Generate executable code from the Simulink model using the `rtwbuild` command from Simulink Coder.

rtwbuild(mdl)

### Starting build procedure for model: mpc_customQPcodegen -->Converting model to discrete time. Assuming no disturbance added to measured output channel #1. -->The "Model.Noise" property of the "mpc" object is empty. Assuming white noise on each measured output channel. ### Successful completion of build procedure for model: mpc_customQPcodegen

On a Windows system, after the build process finishes, the software adds the executable file `mpc_customQPcodegen.exe` to your working folder.

Run the executable. After the executable completes successfully (`status = 0`), the software adds the data file `mpc_customQPcodegen.mat` to your working folder. Load the data file into the MATLAB workspace, and obtain the plant input and output signals generated by the executable.

if ispc status = system(mdl); load(mdl) uDantzigCodeGen = u; yDantzigCodeGen = y; else disp('The example only runs the executable on Windows system.'); end

** starting the model ** ** created mpc_customQPcodegen.mat **

## Compare Simulation Results

Compare the plant input and output signals from all the simulations.

if ispc figure subplot(2,1,1) plot(u.time,uKWIK.signals.values,u.time,uDantzigSim.signals.values,... '+',u.time,uDantzigCodeGen.signals.values,'o') subplot(2,1,2) plot(y.time,yKWIK.signals.values,y.time,yDantzigSim.signals.values,... '+',y.time,yDantzigCodeGen.signals.values,'o') legend('KWIK','Dantzig Simu','Dantzig CodeGen','Location','northwest') else figure subplot(2,1,1) plot(u.time,uKWIK.signals.values,u.time,uDantzigSim.signals.values,'+') subplot(2,1,2) plot(y.time,yKWIK.signals.values,y.time,yDantzigSim.signals.values,'+') legend('KWIK','Dantzig Simu','Location','northwest') end

The signals from all the simulations are identical.

## References

[1] Bemporad, A. and Mosca, E. "Fulfilling hard constraints in uncertain linear systems by reference managing." *Automatica*, Vol. 34, Number 4, pp. 451-461, 1998.

bdclose(mdl)