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
% Add to PHI
PHI = [PHI F];
% Clear F
F = [];
end
end
