# How can I find the best control law for MPC with constraints? Quadratic Programming

The goal with optimal control is to find the best input signals for the system. The goal with optimal control with constraints is to find good input signal who fits the reality. I mean, you cannot drive faster with your car if you press the pedal more over into the buttom, because the bottom is the limit for your input signal for the car's engine.

I want to find the constrained input signal for my system, with use of quadratic programming.

Assume that we have our discrete state space model:

$$x(k+1) = Ax(k) + Bu(k) \\ y(k) = Cx(k) + Du(k)$$

And we want to find the future states and compute a good control law for our system which fits the reality.

We use our prediction matrices.

$$F = \begin{bmatrix} CA\\ CA^2\\ CA^3\\ \vdots \\ CA^{N_p} \end{bmatrix} , \Phi = \begin{bmatrix} CB &0 &0 &\cdots & 0\\ CAB & CB & 0 & \cdots & 0\\ CA^2B& CAB & 0 &\cdots &0 \\ \vdots & \vdots & \vdots & \vdots &\vdots \\ CA^{N_p-1}B & CA^{N_p-2}B & CA^{N_p-3}B & \cdots & CA^{N_p-N_c}B \end{bmatrix}$$

$N_p$ is the predict horizon and $N_c$ is the control horizon. See them as tuning parameters.

They will fit into the equation:

$$Y = Fx_0 + \Phi U$$

Where $Y$ is the future output and $U$ is the future input. Our goal is to find $U$. Also $x_0$ is the current state where we are now. The future inputs looks like this:

$$U = \begin{bmatrix} u(1)\\ u(2)\\ u(3)\\ u(4)\\ \vdots \\ u(k) \end{bmatrix}$$

Where $u(k)$ is a vector with the same dimension as the column length of $B$ The same is for $Y$.

$$Y = \begin{bmatrix} y(1)\\ y(2)\\ y(3)\\ y(4)\\ \vdots \\ y(k) \end{bmatrix}$$

Now we will pick up our cost function $J$ which we will minimize.

$$J = (R_s - Y)^T(R_s - Y) + U^T\bar R U$$

The matrix $\bar R$ is a tuning matrix who look like this:

$$\bar R = r_{\omega} I_{N_p x N_p}$$

Where $I_{N_p x N_p}$ is the square identity matrix of $N_p$ dimension and $r_{\omega}$ is the tuning parameter, not a vector!

$R_s$ is the set point vector. Also called reference vector.

We want to minimize our cost function and we can write that function as:

$$J = (R_s −Fx_0)^T (R_s −Fx_0)-2U^T \Phi^T (R_s -Fx_0)+U^T (\Phi^T \Phi+ \bar R)U$$

To make the $J$ as small as possible, we set $J = 0$. To do that, we need to derive the cost function.

$$\frac{\partial J}{\partial U} = −2\Phi^T (R_s -Fx_0) + 2(\Phi^T \Phi + \bar R)U = 0$$

And to find the best input signals $U$:

$$U = (\Phi^T \Phi + \bar R)^{-1} \Phi^T(R_s - Fx_0))$$

Notice tha we can split $U$ into two parts. From this:

$$U = (\Phi^T \Phi + \bar R)^{-1} \Phi^T(R_s - Fx_0))$$

To this:

$$\boxed{U = (\Phi^T \Phi + \bar R)^{-1} \Phi^T R_s - (\Phi^T \Phi + \bar R)^{-1} \Phi^T Fx_0}$$

In LQR methods, we can directly see that the inputs signals $U$ is exactly as the control law:

$$\boxed{u = K_r r(k) - L x}$$

Were $K_r$ is the precomensator factor, also called feed forward, for the reference vector $r(k)$ and $L$ is our beloved control law and $x$ is the state vector.

Then we can say that:

$$\boxed{K_r = (\Phi^T \Phi + \bar R)^{-1} \Phi^T \bar R_s}$$ $$\boxed{L = (\Phi^T \Phi + \bar R)^{-1} \Phi^T F}$$

Notice that I changed $R_s$ to $\bar R_s$. That's because:

$$R_s = \bar R_s r = \overset{N_p}{\begin{bmatrix} 1\\ 1\\ 1\\ 1\\ 1 \end{bmatrix}}r$$

Where $r$ is our reference vector and $\bar R_s$ is also a vector which will help us to decrease $K_r$.

Then we $m$ rows of $K_r$ and $L$ and $m$ is the dimension of columns of $B$

Now I will talk about quadratic programming. Simply, QP, is a tool which finds the best control law and precompensator factor but QP take into account of the limits of the system.

QP programming minimizes the cost function:

$$J = \frac{1}{2}x^TQx + c^Tx$$

Question:

How do I minimize the cost function: $$J = (R_s −Fx_0)^T (R_s −Fx_0)-2U^T \Phi^T (R_s -Fx_0)+U^T (\Phi^T \Phi+ \bar R)U$$

With quadratic programming? The tuning parameters here will be $N_c, N_p, r_{\omega}$. They control how large $F$ and $\Phi$ will be.

Or do I fist need to compute the optimal signals $U$ and then use QP to change the optimal signals $U$ so the fits the reality?

MATLAB/Octave code below for finding $K_r$ and $L$ without constraints. Requires Matavecontrol toolbox.

% How to find the precompensator factor and control law for MPC
function [Kr, L] = mpctest(varargin)
% Create matricies
delay = 0;
A = [0 1; -30 -3];
B = [0; 1];
C = [1 0];

% Create state space
sys = ss(delay, A, B, C); % D = Automatic

% Convert to discrete
h = 0.03;
sysd = c2d(sys, h)

% Tuning parameters
Np = 10; % Prediction horizon
Nc = 5; % Control horizon
Rw = 0.001; % Tuning parameter

% Compute the F matrix now!
F = Fmatrix(C, A, Np);
% Compute the PHI matrix now
PHI = PHImatrix(C, A, B, Np, Nc);
% Find the MPC control law L
barR = Rw*eye(Nc, Nc);
L = inv(PHI'*PHI+barR)*PHI'*F;
% Get size of B matrix
[n, m] = size(B);
% Get the m rows of L control law
L = L(m, :);
% Find the MPC precompensator factor Kr
barRs = ones(1, Np)';
Kr = inv(PHI'*PHI+barR)*PHI'*barRs;
% Get the m rows
Kr = Kr(m, :);
end

function [F] = Fmatrix(C, A, Np)
F = [];
for i = 1:(Np)
F = [F; C*A^i];
end

end

function [PHI] = PHImatrix(C, A, B, Np, Nc)
F = [];
PHI = [];
for j = 1:Nc
for i = (1-j):(Np-j)

if i < 0
F = [F; 0*C*A^i*B];
else
F = [F; C*A^i*B];
end

end

PHI = [PHI F];
% Clear F
F = [];
end

end


If you simply want to experiment with MPC easily and not bother about how the resulting optimization problems actually are solved deep inside the solver (normally you would not care about that, just use a good off-the-shelf solver), you might want to use a modelling tool such as MPT or YALMIP (developed by me). Here are some examples to get started https://yalmip.github.io/example/standardmpc/

• I just want to minimize the cost function for MPC: $$J = (R_s -Fx(k_i ))^T (R_s -Fx(k_i ))-2U^T\Phi^T(R_s -Fx(k_i ))+U^T (\Phi^T \Phi + \bar R)U$$ by using QP. I also don't have MATLAB.
– DanM
Apr 1, 2018 at 15:57
• What I really want, is to learn how to build a controller who computes the inputs. A MPC with constraints would be good to learn. They are much better that LQR.
– DanM
Apr 1, 2018 at 16:06
• I hope you mean you want to minimize it with constraints also. Otherwise the optimal solution is simply given by the stationary conditions just like you showed. YALMIP works in Octave too. If you have neither, you simply have to use a QP solver in whatever language you are using and accept that you probably will have to do some messy coding to actually setup the numerical matrices that define the QP for an MPC problem. QP solvers implementations are available in most languages. Apr 1, 2018 at 16:18
• Yes I want to learning how to minimize a cost function for LQR/MPC with taking account of the limits in the system. E.g constraints. But I having much trouble to find $Q$ and $c$ matricies for QP cost formula $$J = \frac {1}{2}x^TQA + c^Tx$$ by looking at the LQR/MPC cost function.
– DanM
Apr 1, 2018 at 16:50
• In your model $Q = (\Phi^T\Phi + \bar{R})$ and $c = -(R_s - Fx_0)^T\Phi$ Apr 1, 2018 at 17:02

In order to write the cost function

$$J = (R_s − Fx_0)^\top (R_s −F\,x_0) - 2\,U^\top \Phi^\top (R_s - F\,x_0) + U^\top (\Phi^\top \Phi + \bar{R})\,U$$

into the following form

$$J = \frac{1}{2}x^\top Q\,x + c^\top x$$

such that it can be minimized using quadratic programming, you first have to realize that when performing minimization it does not matter if you want to minimize $J(z)$ with respect to $z$ or $J(z)+\alpha$ with respect $z$, where $\alpha$ is just some constant. In the first and second equation the design variables are $U$ and $x$ respectively. From here it is straightforward to identify $Q$ and $c$ by finding the same quadratic and linear terms in $U$, which gives $Q = 2\,\left(\Phi^\top \Phi + \bar{R}\right)$ and $c = 2\,\Phi^\top (F\,x_0 - R_s)$. The remaining term $(R_s − Fx_0)^\top (R_s −F\,x_0)$ is just a constant, so as stated before can be ignored for the minimization.

These values for $Q$ and $c$ can be simplified a little by factoring out the common factor $2$, since minimizing $J(z)$ with respect to $z$ is the same as minimizing $\beta\,J(z)$ with respect to $z$, where $\beta$ is some positive constant. So $Q = \Phi^\top \Phi + \bar{R}$ and $c = \Phi^\top (F\,x_0 - R_s)$ could be used as well.

• Shouldn't that be $$Q = (\Phi^\top \Phi + \bar{R})$$ and $$c = - 2\, \Phi^\top (R_s - F\,x_0)$$ ? Where did you get number 2 from in $Q$ matrix?
– DanM
Apr 1, 2018 at 18:59
• Sorry! I know now why you add 2 at the $Q$ matrix. But the $c$ matrix should be negative?
– DanM
Apr 1, 2018 at 19:32
• @DanielMårtensson because there is a half in front of the quadratic term in the definition for the cost function with $Q$ and $c$. So that factor two cancels with the factor half. Apr 1, 2018 at 19:34
• So I cannot use $$Q = 2(\Phi^T\Phi +\bar R)$$ ? I need to use $$Q = \Phi^T\Phi +\bar R$$ and also the matrix $c$ cannot be $$c = - 2\Phi^T(R_s - Fx_0)$$. The matrix $c$ need to be $$c = \Phi^T(R_s -Fx_0)$$ ?
– DanM
Apr 1, 2018 at 19:47
• @DanielMårtensson the minus sign in front of the $c$ term was pulled into the brackets (look at the order of $F\,x_0$ and $R_s$). And as I stated in my answer the common factor of two in both $Q$ and $c$ can be removed if you want to. Apr 1, 2018 at 20:10

How to solve a QP?

Here are the options:

• Interior point method [1]

• Exterior point method [2], [3]

• Sequential quadratic programming (SQP) [2]

• Active set strategy [4]

There is also an out of box tool quadprog for Octave and Matlab.

This is to obtain the optimal input.

To optimize the tuning of prediction and control horizons $N_c$ and $N_p$, GA is one of solutions [7].