6. MathWorks Linear MPC Plugin

As a result of a long-term collaboration, MathWorks Inc. and embotech AG developed a MATLAB® plugin for FORCESPRO. Users are now able to use the FORCESPRO solver in MATLAB® and Simulink® from within the MATLAB® Model Predictive Control Toolbox. The plugin leverages the powerful design capabilities of the Model Predictive Control Toolbox™ and the computational performance of FORCESPRO. With FORCESPRO 2.0, toolbox users can now easily define challenging control problems and solve long-horizon MPC problems more efficiently.

Model Predictive Control Toolbox™ provides functions, an app, and Simulink® blocks for designing and simulating model predictive controllers. The toolbox enables users to readily specify plant and disturbance models, horizons, constraints, and weights. User-friendly control design capabilities of Model Predictive Control Toolbox™, combined with the powerful numerical algorithms of FORCESPRO, enables code deployment of the FORCESPRO solver on real-time hardware from within MATLAB® and Simulink®, in addition to the QP solvers shipped by MathWorks. The new FORCESPRO interface comes with various features such as Simulink blocks that can generate code runnable on embedded targets such as dSpace. The parameters of the MPC algorithm, such as plant and disturbance model, prediction horizon, constraints and move-blocking strategy can be specified directly. The toolbox enables users to run closed-loop simulations and evaluation of controller performance. User-friendly MPC design capabilities are combined with the powerful numerical algorithms of FORCESPRO. This combination of the Model Predictive Control Toolbox™ and FORCESPRO enables code deployment on real-time hardware. The generated code is highly optimized for fast computations and low memory footprint.

This interface is provided with all variants of FORCESPRO, starting with Variant S. It is compatible with MATLAB R2019b, 2020a and 2020b.

The plugin mainly consists of the three following MATLAB commands which are described in details in this chapter:

  • mpcToForces for generating a FORCESPRO solver from an MPC object designed by the Model Predictive Control Toolbox

  • mpcmoveForces for calling the generated solver on a specific MPC problem instance

  • mpcCustomSolver for using the FORCESPRO dense QP solver as a custom solver

An auxiliary file is also exposed to the users for generating different solvers options, namely mpcToForcesOptions.

The following LTI MPC features are supported:

  • Continuous and discrete time plant models

  • Move blocking

  • Measured disturbances

  • Unmeasured disturbances

  • Disturbance and noise models

  • Uniform or time-varying weights on outputs, manipulated variables, manipulated variables rates and a global slack variable

  • Uniform or time-varying bounds on outputs, manipulated variables and manipulated variables rates

  • Soft constraints

  • Signal previewing on reference and measured disturbances

  • Scale factors

  • Nominal values

  • Online updates of weights and constraints

  • Built-in and custom state estimators

Currently, convex quadratic programs are supported by the MATLAB plugin. Extensions to adaptive and linear time-varying are under development. The current limitations of the plugin are the following:

  • Mixed input-output constraints are not covered

  • Offdiagonal terms on the hessian of the objective cannot be implemented

  • Unconstrained problems are not supported

  • No single-precision solvers, only double precision currently

  • No suboptimal solutions

6.1. Different types of solvers

The plugin converts an MPC object (weights, bounds, horizons, prediction model) into a quadratic program (QP) formulated via the FORCESPRO API. One key design decision is to choose the decision variables in the quadratic program. There are two classic choices and they lead to two different formulations:

  • Dense QP, where only the manipulated variables \(MV\) or \(\Delta{}MV\) are decision variables. In this case, the hessian and linear constraints matrices are stored as dense matrices.

  • Sparse QP, where \(MV\), \(\Delta{}MV\), the outputs \(OV\) and the states \(X\) are decision variables. In this case, all matrices have a block sparse structure as in Low-level interface.

Typically, a dense QP has fewer optimization variables, zero equality constraints and many inequality constraints. Although the sparse QP is generally much larger than the dense QP its structure can be efficiently exploited to reduce the solve times. Besides, the dense formulation has an inherent flaw, which is that the condition number increases with the horizon length, especially when the plant states have large contributions to the plant inputs and outputs. Thus, the best solution is to allow users to switch to the sparse formulation, which prevents numerical blow-ups when the plant is unstable. Nevertheless, the dense formulation can be beneficial in terms of solve time when there is an important amount of move-blocking.

6.2. Different algorithms

In addition to the general QP solver, the FORCESPRO plugin for the MPC toolbox supports also supports a fast QP solver from FORCESPRO version 6.0.0. Both the general and the fast algorithms are supported for the two different solver types documented in Different types of solvers. The workflow for the general QP solver is exactly as described in the following sections. The difference in workflow for the fast QP solver is an additional “tuning” step which will be explained here. The core idea behind this tuning step is that it allows for a highly specialized/tailored QP solver which achieves optimal performance for a given MPC application. For this purpose FORCESPRO ships with a caching tool for collecting simulation data in order to tune the fast QP solver. The caching tool is a simple mechanism and interacting with it goes via three commands:

  • forcesMpcCache clear: Deletes all stored caches completely.

  • forcesMpCache on: Turns on the cache by constructing a cache object. This command must be called before mpcToForces is called in order to properly cache simulation data. After the cache has been turned on and until it is turned off every call to the generated solver via mpcmoveForces will store a problem instance in the cache, so it can later be used for tuning the fast QP solver.

  • forcesMpcCache off: Stores the allocated cache in a folder named “FORCESPRO_CACHE” and deletes the cache object from the base workspace.

  • forcesMpcCache reset: A single command for running forcesMpcCache clear; forcesMpcCache on.

When no cache has been stored (default) a general QP solver is generated when calling mpcToForces. If on the other hand a cache is available when calling mpcToForces, then a fast QP solver is generated. Hence, after constructing and specifying a MPC object mpcobj, the complete workflow for generating a fast QP solver is as follows:

  1. Turn on the FORCESPRO cache by running forcesMpcCache on (or forcesMpcCache reset) in the MATLAB terminal.

  2. Generate a general QP solver by calling mpcToForces(mpcobj,options) (see Generating a QP solver from an MPC object).

  3. Run a simulation, using mpcmoveForces to compute optimal control moves. It is important that mpcmoveForces is called as opposed to the generated MEX function, as that one will not be able to cache the problem instances which are needed for tuning the fast QP solver.

  4. Turn off the cache by running forcesMpcCache off in the MATLAB terminal.

  5. Generate a QP fast solver by calling mpcToForces(mpcobj,options) (see Generating a QP solver from an MPC object). Since the cache has been stored in step 4, this will trigger the automatic tuning procedure.

For examples displaying the workflow for the fast QP solver, see our motor example or our cstr example.

Important

If the MATLAB command clear is called in between the calls forcesMpcCache on and forcesMpcCache off the cache is deleted. Hence, this should be avoided.

6.3. Generating a QP solver from an MPC object

Given an MPC object created by the mpc command, users can generate a QP solver tailored to their specific problem via the following command:

% mpcobj is the output of mpc(...)
% options is the output of mpcToForcesOptions(...)

[coredata, statedata, onlinedata] = mpcToForces(mpcobj, options);

Two types of QP solvers can be generated via mpcToForces: a sparse solver that corresponds to a multi-stage formulation as in Low-level interface and a dense solver that corresponds to a one-stage QP with inequality constraints only.

The API of mpcToForces is described in more details in the tables below. The mpcToForces command expects an MPC object mpcobj and a structure options generated by mpcToForcesOptions as inputs.

Table 6.1 mpcToForces inputs

Input

Description

mpcobj

LTI MPC controller designed by Model Predictive Control Toolbox

options

Object that provides solver generation options.

The outputs of mpcToForces consist of three structures coredata, statedata and onlinedata. The FORCESPRO server generates two types of solvers:

  • customForcesSparseQP when the option 'sparse' is set. An m file named `customForcesSparseQP.m` with the corresponding mex interface as well as the solver libraries and header in the `customForcesSparseQP` folder. In this particular case (sparse), the name of the solver can be set by users.

  • customForcesDenseQP when the option 'dense' is set. An m file named `customForcesDenseQP.m` with the corresponding mex interface as well as the solver libraries and header in the `customForcesDenseQP` folder. In this particular case (dense), the solver name cannot be changed by users.

Table 6.2 mpcToForces outputs

Output

Type

Description

coredata

Structure

Stores constant Store constant data needed to construct quadratic progam at run-time

statedata

Structure

Represents prediction model states and last optimal MV.

The index \(k\) stands for the current simulation time.

It contains 4 fields:

When built-in state estimation is used:

Plant is the estimated plant state \(x_p[k|k-1]\)

Disturbance is the estimated disturbance states \(x_d[k|k-1]\)

Noise is the estimated measurement noise states \(x_n[k|k-1]\)

LastMove is the optimal manipulated variables at the previous sample time

In this case, users should not manually change any field at run-time.

When custom state estimation is used:

Plant is the estimated plant state \(x_p[k|k]\)

Disturbance is the estimated disturbance states \(x_d[k|k]\)

Noise is the estimated noise states \(x_n[k|k]\)

LastMove is the optimal manipulated variables at the previous solve

In this case, user should manually update Plant, Disturbance (if used), Noise (if used) fields at run-time but leave LastMove alone.

onlinedata

Structure

Represent online signals

It contains up to three fields:

signals, a structure containing following fields:

ref (references of Output Variables)

mvTarget (references of Manipulated Variables)

md (when Measured Disturbance is present)

ym (when using the built-in estimator)

externalMV (when UseExternalMV is true in the options object)

weights, a structure containing the following fields:

y (when UseOnlineWeightOV is enabled)

u (when UseOnlineWeightMV is enabled)

du (when UseOnlineWeightMVRate is enabled)

ecr (when UseOnlineWeightECR is enabled)

constraints, a structure containing the following fields:

vmin (when UseOnlineConstraintOVMin is enabled)

vmax (when UseOnlineConstraintOVMax is enabled)

umin (when UseOnlineConstraintMVMin)

umax (when UseOnlineConstraintMVMax)

dumin (when UseOnlineConstraintMVRateMin)

dumax (when UseOnlineConstraintMVRateMax)

In order to provide the code-generation options to mpcToForces, the user needs to run the command mpcToForcesOptions with one of the following two arguments as input:

  • “dense” for generating the options of a one-stage dense QP solvers

  • “sparse” for generating the options a multi-stage QP solver.

The structures provided by the mpcToForcesOptions command have the following MPC related fields in common between the “dense” and “sparse” case:

  • SkipSolverGeneration. When set to True, only structures are returned. If set to False, a solver mex interface is generated and the structures are returned. Default value is False.

  • UseOnlineWeightOV. When set to True, it allows Output Variables weights to vary at run time. Default is False.

  • UseOnlineWeightMV. When set to True, it allows Manipulated Variables weights to vary at run time. Default is False.

  • UseOnlineWeightMVRate. When set to True, it allows weights on the Manipulated Variables rates to vary at run time. Default is False.

  • UseOnlineWeightECR. When set to True, it allows weights on the ECR to change at run time. Default is False.

  • UseOnlineConstraintOVMax. When set to True, it allows updating the upper bounds on Output Variables at run time. Default is False.

  • UseOnlineConstraintOVMin. When set to True, it allows updating the lower bounds on Output Variables at run time. Default is False.

  • UseOnlineConstraintMVMax. When set to True, it allows updating the upper bounds on Manipulated Variables at run time. Default is False.

  • UseOnlineConstraintMVMin. When set to True, it allows updating the lower bounds on Manipulated Variables at run time. Default is False.

  • UseExternalMV. When set to True, the actual Manipulated Variable applied to the plant at time \(k-1\) is provided as output. Default is False.

  • UseMVTarget. When set to True, an MV reference signal is provided via the onlinedata structure. In this case, MV weights should be positive for proper tracking. When false, the MV reference is the nominal value by default and MV weights should be zero to avoid unexpected behaviour. Default is False.

Both the “dense” and “sparse” options structures have the following solver related fields in common:

  • ForcesServer is the FORCESPRO server url. Default is forces.embotech.com.

  • ForcesMaxIteration is the maximum number of iterations in a FORCESPRO solver. Default value is \(50\).

  • ForcesPrintLevel is the logging level of the FORCESPRO solver. If equal to \(0\), there is no output. If equal to \(1\), a summary line is printed after each solve. If equal to \(2\), a summary line is printed at every iteration. Default value is 0.

  • ForcesInitMethod is the initialization strategy used for the FORCESPRO interior point algorithm. If equal to \(0\), the solver is cold-started. If equal to \(1\), a centered start is computed. Default value is \(1\).

  • ForcesMu0 is the initial barrier parameter. It must be finite and positive. Its default value is equal to \(10\). A small value close to \(0.1\) generally leads to faster convergence but may be less reliable.

  • ForcesTolerance is the tolerance on the infinity norm of the residuals of the inequality constraints. It must be positive and finite. Its default value is \(10^{-6}\).

  • ForcesTargetPlatform for choosing a target platform to deploy the solver. Currently, dSpace, Speedgoat, BeagleBone-Blue and AURIX are supported.

In the “sparse” solver case, there are four more fields:

  • SolverName for customuzing the solver name.

  • UseOnlineConstraintMVRateMax for setting MVRate upper bounds.

  • UseOnlineConstraintMVRateMin for setting MVRate lower bounds.

  • UseOneSlackVariablePerStep to enable one slack variable per prediction step.

6.4. Solving a QP from MPC online data

Once a QP solver has been generated it can be used to solve online MPC problems via the MATLAB command mpcmoveForces as follows

% the coredata, statedata and onlinedata structures are outputs of mpcToForces

[mv,statedata,info] = mpcmoveForces(coredata,statedata,onlinedata);

The outputs of the mpcmoveForces command are described below. In the table below \(n_m\) denotes the number of manipulated variables, \(n_x\) stands for the state dimension of the system implemented in the MPC object, \(p\) is the prediction horizon and \(k\) is the current solve time instant.

Table 6.3 mpcmoveForces outputs

Output

Type

Description

mv

Vector of size nm

Optimal manipulated variables at current solve time instant

statedata

Structure

Initialized by mpcToForces

info

Structure

Information about the FORCESPRO solve

Uopt is a \(p\times{}n_m\) matrix for the optimal manipulated variables over the prediction horizon \(k\) to \(k+p-1\)

Yopt is a \(p\times{}n_y\) matrix for the optimal output variables over the prediction horizon \(k+1\) to \(k+p\)

Xopt is a \(p\times{}n_x\) matrix for the optimal state variables over the prediction horizon \(k+1\) to \(k+p\)

Slack is a \(p\times{}1\) vector of slack variables

Exitflag is the FORCESPRO solve exit flag. If it is equal to 1, an optimal solution has been found. If it is equal to 0, the maximum number of solver iterations has been reached. A negative flag means that the solver failed to find a feasible solution.

Iterations is the number of solver iterations upon convergence or failure

Cost is the cost returned by the solver

6.7. Examples

The plugin comes with several examples to demonstrate its functionalities and flexibility.

The packaged examples are the following ones:

  • forcesmpc_cstr.m is a linear time-invariant (LTI) MPC example with unmeasured outputs. It also shows how to use the MATLAB Coder for generating and running mpcmoveForces as a mex interface, which results in lower simulation times.

    Code is available here.

  • forcesmpc_targets.m is an LTI MPC example with a reference on one manipulated variables

    Code is available here.

  • forcesmpc_preview.m is an LTI MPC example with previewing on the output reference and the measured disturbance

    Code is available here.

  • forcesmpc_motor.m is an LTI MPC example with state and input constraints

    Code is available here.

  • forcesmpc_miso.m is an LTI MPC example with one measured output, one manipulated variable, one measured disturbance, and one unmeasured disturbance

    Code is available here.

  • forcesmpc_simplelti.m demonstrates a simple LTI MPC designed

    Code is available here.

  • forcesmpc_linearize.m is an example of linear MPC around an operating point of a nonlinear system.

    Code is available here.

  • forcesmpc_customqp.m shows how to use the FORCESPRO dense QP solver as a custom solver in an MPC object

    Code is available here.

  • forcesmpc_onlinetuning.zip demonstrates how to run the MPC Simulink block.

    Code is available here.

  • forcesmpc_onlinetuning_dSpace_MicroAutoBoxII.zip demonstrates how to generate code for dSpace MicroAutoBox II using the MPC Simulink block.

    Code is available here.

The forcesmpc_linearize.m example is described in more details below. First, the linearized model and the operating point are loaded from a MAT file.

%% Load plant model linearized at its nominal operating point (x0, u0, y0)
load('nomConditionsLinearize.mat');

An MPC controller object is then created with a prediction horizon of length \(p = 20\), a control horizon \(m = 3\) and a sampling period \(T_s = 0.1\) seconds as explained here.

%% Design MPC Controller
% Create an MPC controller object with a specified sample time |Ts|,
% prediction horizon |p|, and control horizon |m|.
Ts = 0.1;
p = 20;
m = 3;
mpcobj = mpc(plant,Ts,p,m);

Nominal values need to be set in the MPC object.

% Set the nominal values in the controller.
mpcobj.Model.Nominal = struct('X',x0,'U',u0,'Y',y0);

Constraints are set on the manipulated variables and an output reference signal is provided.

% Set the manipulated variable constraint.
mpcobj.MV.Max = 0.2;

% Specify the reference value for the output signal.
r0 = 1.5*y0;

From the MPC object and a structure of options, a FORCESPRO solver can be generated.

% Create options structure for the FORCESPRO sparse QP solver
options = mpcToForcesOptions();
% Generates the FORCESPRO QP solver
[coredata, statedata, onlinedata] = mpcToForces(mpcobj, options);

Once a reference signal has been constructed, the simulation can be run using mpcmoveForces.

for t = 1:Tf
  % A measurement noise is simulated
  Y(:, t) = dPlant.C * (X(:, t) - x0) + dPlant.D * (U(:, t) - u0) + y0 + 0.01 * randn;
  % Prepare inputs of mpcmoveForces
  onlinedata.signals.ref = r(t:min(t+mpcobj.PredictionHorizon-1,Tf),:);
  onlinedata.signals.ym = Y(:, t);
  % Call FORCESPRO solver
  [mv, statedata, info] = mpcmoveForces(coredata, statedata, onlinedata);
  if info.ExitFlag < 0
    warning('Internal problem in FORCESPRO solver');
  end
  U(:, t) = mv;
  X(:, t+1) = dPlant.A * (X(:, t) - x0) + dPlant.B * (U(:, t) - u0) + x0;
end

The resulting input and output signals are shown in Figure Figure 6.23 and Figure Figure 6.24 respectively.

../_images/plugin_linearize_u.png

Figure 6.23 Manipulated variable computed by the FORCESPRO plugin.

../_images/plugin_linearize_y.png

Figure 6.24 Output variable computed by the FORCESPRO plugin.