Documentation

## Solve Custom MPC Quadratic Programming Problem and Generate Code

This example shows how to use the built-in QP solver to implement a custom MPC control algorithm that supports C code generation in MATLAB.

### Define Plant Model

The plant model is a discrete-time state-space system and it is open-loop unstable. We assume that all the plant states are measurable. Therefore, we avoid the need for designing a state estimator, which is beyond the scope of this example.

```A = [1.1 2; 0 0.95]; B = [0; 0.0787]; C = [-1 1]; D = 0; Ts = 1; sys = ss(A,B,C,D,Ts); x0 = [0.5;-0.5]; % initial states at [0.5 -0.5] ```

### Design Unconstrained Linear Quadratic Regulator (LQR)

Design an unconstrained LQR with output weighting. This controller serves as the baseline to compare with the custom MPC algorithm. The LQ control law is u(k) = -K_lqr*x(k).

```Qy = 1; R = 0.01; K_lqr = lqry(sys,Qy,R); ```

Run a simulation with initial states at [0.5 -0.5]. The closed-loop response is stable.

```t_unconstrained = 0:1:10; u_unconstrained = zeros(size(t_unconstrained)); Unconstrained_LQR = tf([-1 1])*feedback(ss(A,B,eye(2),0,Ts),K_lqr); lsim(Unconstrained_LQR,'-',u_unconstrained,t_unconstrained,x0); hold on; ``` ### Design Custom MPC Controller with Terminal Weight

Design a custom MPC controller with the terminal weight applied at the last prediction step.

The predicted state sequences, X(k), generated by the linear model and input sequence, U(k), can be formulated as: X(k) = M*x(k) + CONV*U(k). In this example, use four prediction steps (N = 4).

```M = [A;A^2;A^3;A^4]; CONV = [B zeros(2,1) zeros(2,1) zeros(2,1);... A*B B zeros(2,1) zeros(2,1);... A^2*B A*B B zeros(2,1);... A^3*B A^2*B A*B B]; ```

The MPC objective function is J(k) = sum(x(k)'*Q*x(k) + u(k)'*R*u(k) + x(k+N)'*Q_bar*x(k+N)). To ensure that the MPC objective function has the same quadratic cost as the infinite horizon quadratic cost used by LQR, terminal weight Q_bar is obtained by solving the following Lyaponov equation:

```Q = C'*C; Q_bar = dlyap((A-B*K_lqr)', Q+K_lqr'*R*K_lqr); ```

Convert the MPC problem into a standard QP problem, which has the objective function J(k) = U(k)'*H*U(k) + 2*x(k)'*F'*U(k).

```Q_hat = blkdiag(Q,Q,Q,Q_bar); R_hat = blkdiag(R,R,R,R); H = CONV'*Q_hat*CONV + R_hat; F = CONV'*Q_hat*M; ```

When there are no constraints, the optimal predicted input sequence U(k) generated by MPC controller is -K*x, where K = inv(H)*F.

```K = H\F; ```

In practice, only the first control move u(k) = -K_mpc*x(k) is applied to the plant (receding horizon control).

```K_mpc = K(1,:); ```

Run a simulation with initial states at [0.5 -0.5]. The closed-loop response is stable.

```Unconstrained_MPC = tf([-1 1])*feedback(ss(A,B,eye(2),0,Ts),K_mpc); lsim(Unconstrained_MPC,'*',u_unconstrained,t_unconstrained,x0) legend show ``` LQR and MPC controllers produce the same result because the control laws are the same.

```K_lqr K_mpc ```
```K_lqr = 4.3608 18.7401 K_mpc = 4.3608 18.7401 ```

### LQR Control Performance Deteriorates When Applying Constraints

Restrict the controller output, u(k), to be between -1 and 1. The LQR controller generates a slow and oscillatory closed-loop response due to saturation.

```x = x0; t_constrained = 0:40; for ct = t_constrained uLQR(ct+1) = -K_lqr*x; uLQR(ct+1) = max(-1,min(1,uLQR(ct+1))); x = A*x+B*uLQR(ct+1); yLQR(ct+1) = C*x; end figure subplot(2,1,1) plot(t_constrained,uLQR) xlabel('time') ylabel('u') subplot(2,1,2) plot(t_constrained,yLQR) xlabel('time') ylabel('y') legend('Constrained LQR') ``` ### MPC Controller Solves QP Problem Online When Applying Constraints

One of the major benefits of using MPC controller is that it handles input and output constraints explicitly by solving an optimization problem at each control interval.

Use the built-in KWIK QP solver, `mpcqpsolver`, to implement the custom MPC controller designed above. The constraint matrices are defined as Ac*x>=b0.

```Ac = -[1 0 0 0;... -1 0 0 0;... 0 1 0 0;... 0 -1 0 0;... 0 0 1 0;... 0 0 -1 0;... 0 0 0 1;... 0 0 0 -1]; b0 = -[1;1;1;1;1;1;1;1]; ```

The `mpcqpsolver` function requires the first input to be the inverse of the lower-triangular Cholesky decomposition of the Hessian matrix H.

```L = chol(H,'lower'); Linv = L\eye(size(H,1)); ```

Run a simulation by calling `mpcqpsolver` at each simulation step. Initially all the inequalities are inactive (cold start).

```x = x0; iA = false(size(b0)); opt = mpcqpsolverOptions; opt.IntegrityChecks = false; for ct = t_constrained [u, status, iA] = mpcqpsolver(Linv,F*x,Ac,b0,[],zeros(0,1),iA,opt); uMPC(ct+1) = u(1); x = A*x+B*uMPC(ct+1); yMPC(ct+1) = C*x; end figure subplot(2,1,1) plot(t_constrained,uMPC) xlabel('time') ylabel('u') subplot(2,1,2) plot(t_constrained,yMPC) xlabel('time') ylabel('y') legend('Constrained MPC') ``` The MPC controller produces a closed-loop response with faster settling time and less oscillation.

### Simulate Custom MPC Using MATLAB Function Block in Simulink

`mpcqpsolver` can be used inside a MATLAB Function block to provide simulation and code generation in the Simulink environment.

```mdl = 'mpc_customqp'; open_system(mdl) ``` The Custom MPC Controller block is the MATLAB Function block. Double-click it to examine the MATLAB code. Since `Linv`, `F`, `Ac`, `b0` matrices, and `opt` structure are constant, they are passed into the MATLAB Function block as parameters.

Run a simulation in Simulink. The closed-responses of LQR and MPC controllers are identical to their counterparts in the MATLAB simulation.

```open_system([mdl '/u_lqr']) open_system([mdl '/y_lqr']) open_system([mdl '/u_mpc']) open_system([mdl '/y_mpc']) sim(mdl) ```    ### Code Generation in MATLAB

`mpcqpsolver` supports C code generation with MATLAB Coder. Assume you have a function `mycode` that is compatible with the code generation standard.

```function [x,iter,iA1,lam] = mycode() %#codegen n = 5; m = 10; q = 2; H = diag(10*rand(n,1)); f = randn(n,1); A = randn(m,n); b = randn(m,1); Aeq = randn(q,n); beq = randn(q,1); Linv = chol(H,'lower')\eye(n); iA = false(m,1); Opt = mpcqpsolverOptions(); [x,iter,iA1,lam] = mpcqpsolver(Linv,f,-A,-b,Aeq,beq,iA,Opt); ```

You can use following command to generate C code with MATLAB Coder:

```fun = 'mycode'; Cfg = coder.config('mex'); % or 'lib', 'dll', etc. codegen('-config',Cfg,fun,'-o',fun); ```

### Acknowledgment

This example is inspired by Professor Mark Cannon's lecture notes for the Model Predictive Control class at University of Oxford. The plant model is the same one used in Example 2.1 in the "Prediction and optimization" section.

```bdclose(mdl) ```

## Related Topics

#### Implementing an Adaptive Cruise Controller with Simulink

Download technical paper