# How can I minimize the LQR cost function?

LQR and MPC is equal if the MPC controller has no constraints(e.g saturation on inputs/outputs).

But LQR is closed loop feedback by using the LQR control law matrix. MPC is open loop, which means no feedback system.

The LQR control law matrix is created by the Riccati equations and MPC is created by minimize the LQR const fuction.

I wonder if this is the rigth method to use to minimize the LQR const function. First I get my discrete state space model:

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

MPC is a LQR with prediction and prediction means that I want to find the future state if I know the initial state $x_0$. I can show you by summing all the states. Assume that we are at state $x$ and we want to find state $x_n$.

$$x_2 = Ax + Bu_1 \\ x_3 = Ax_2 + Bu_2 = A[Ax + Bu_1] + Bu_2\\ x_4 = Ax_3 + Bu_3 = A[A[Ax + Bu_1] + Bu_2] + Bu_3\\ x_5 = Ax_4 + Bu_4 = A[A[A[Ax + Bu_1] + Bu_2] + Bu_3] + Bu_4\\ x_6 = Ax_5 + Bu_5 = A[A[A[A[Ax + Bu_1] + Bu_2] + Bu_3] + Bu_4] + Bu_5\\ x_7 = Ax_6 + Bu_6 = A[A[A[A[A[Ax + Bu_1] + Bu_2] + Bu_3] + Bu_4] + Bu_5] + Bu_6$$

This will result:

$$x_7 = A^6x + A^5Bu_1 + A^4Bu_2 + A^3Bu_3 + A^2Bu_4 + ABu_5 + Bu_6$$

And it can be expressed as:

$$x_n = A^{n-1}x_0 + \sum_{k = 0}^{n-2} A^{k}Bu_{n-k-1}$$

Where $n$ is the n-step prediction. So if we are at state $x_0$ and we want to find the state $x_n$, we can find it if we know the inputs $u_{n-k-1}$. And this is why MPC is about, finding the inputs $u_{n-k-1}$ to reach the state $x_n$.

Now we want to minimize the quadratic cost function $J$:

$$J =\sum_{k=0}^{n}(x_k^TQx_k + u_k^TRu_k)$$

Where $Q, R$ are tuning matrices. The cost function $J$ can be expressed as:

$$J = \begin{bmatrix} x_0\\ x_1\\ x_2\\ x_3\\ \vdots\\ x_n \end{bmatrix}^T\begin{bmatrix} Q & 0 & 0& 0 & \dots & 0\\ 0 & Q & 0 & 0 & \dots & 0 \\ 0 & 0 &Q & 0 & \dots & 0 \\ 0 & 0 & 0 & Q & \dots & 0 \\ \vdots & \vdots & \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & 0 & 0&\dots & Q \end{bmatrix}\begin{bmatrix} x_0\\ x_1\\ x_2\\ x_3\\ \vdots\\ x_n \end{bmatrix} + \begin{bmatrix} u_0\\ u_1\\ u_2\\ u_3 \\ \vdots\\ u_n \end{bmatrix}^T\begin{bmatrix} R & 0 & 0& 0 & \dots & 0\\ 0 & R & 0 & 0 & \dots & 0 \\ 0 & 0 &R & 0 & \dots & 0 \\ 0 & 0 & 0 & R & \dots & 0 \\ \vdots & \vdots & \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & 0 & 0 & \dots & R \end{bmatrix}\begin{bmatrix} u_0\\ u_1\\ u_2\\ u_3 \\ \vdots\\ u_n \end{bmatrix}$$

And we do the same with $$x_n = A^{n-1}x_0 + \sum_{k = 0}^{n-2} A^{k}Bu_{n-k-1}$$

Express:

$$\begin{bmatrix} x_0\\ x_1\\ x_2\\ x_3\\ \vdots\\ x_{n} \end{bmatrix} = \begin{bmatrix} A\\ A^2\\ A^3\\ A^4\\ \vdots\\ A^{n-1} \end{bmatrix}x_0+\begin{bmatrix} B & 0 & 0 & 0 & \dots & 0\\ AB & B & 0 & 0 & \dots & 0\\ A^2B & AB & B & 0 & \dots& 0\\ A^3B & A^2B & AB & B & \dots& 0\\ \vdots & \vdots & \vdots &\ddots &\ddots & 0\\ A^{n-2}B & A^{n-3}B & A^{n-4}B & A^{n-5}B & \dots & B \end{bmatrix} \begin{bmatrix} u_0\\ u_1\\ u_2\\ u_3\\ \vdots\\ u_n \end{bmatrix}$$

Now! say that:

$$\bar Q = \begin{bmatrix} Q & 0 & 0& 0 & \dots & 0\\ 0 & Q & 0 & 0 & \dots & 0 \\ 0 & 0 &Q & 0 & \dots & 0 \\ 0 & 0 & 0 & Q & \dots & 0 \\ \vdots & \vdots & \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & 0 & 0&\dots & Q \end{bmatrix}$$

$$\bar R = \begin{bmatrix} R & 0 & 0& 0 & \dots & 0\\ 0 & R & 0 & 0 & \dots & 0 \\ 0 & 0 &R & 0 & \dots & 0 \\ 0 & 0 & 0 & R & \dots & 0 \\ \vdots & \vdots & \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & 0 & 0 & \dots & R \end{bmatrix}$$

$$\bar S = \begin{bmatrix} B & 0 & 0 & 0 & \dots & 0\\ AB & B & 0 & 0 & \dots & 0\\ A^2B & AB & B & 0 & \dots& 0\\ A^3B & A^2B & AB & B & \dots& 0\\ \vdots & \vdots & \vdots &\ddots &\ddots & 0\\ A^{n-2}B & A^{n-3}B & A^{n-4}B & A^{n-5}B & \dots & B \end{bmatrix}$$

$$\bar T = \begin{bmatrix} A\\ A^2\\ A^3\\ A^4\\ \vdots\\ A^{n-1} \end{bmatrix}$$

$$z = \begin{bmatrix} u_0\\ u_1\\ u_2\\ u_3\\ \vdots\\ u_n \end{bmatrix}$$

When we can express our cost function $J$ as:

$$J = ( \bar Tx_0 +\bar Sz )^T\bar Q (\bar Tx_0 +\bar Sz ) + z^T\bar R z$$

What I have done is that I have expressed the cost function, without the sum function.

To find $z$, we need to set $J = 0$.

Question:

How can I find input signal vector $z$ which contains the signals $u$.

When expanding the cost function and collect similar terms you get

$$J = x_0^\top \bar{T}^\top \bar{Q}\,\bar{T}\,x_0 + x_0^\top \bar{T}^\top \bar{Q}\,\bar{S}\,z + z^\top \bar{S}^\top\bar{Q}\,\bar{T}\,x_0 + z^\top \left(\bar{S}^\top\bar{Q}\,\bar{S} + \bar{R}\right) z.$$

Since $x_0$ is given and can't be changed, therefore minimizing the cost function above is equivalent to minimizing the following cost function

$$J = f^\top z + z^\top H\,z$$

with

$$f^\top = 2\,x_0^\top \bar{T}^\top \bar{Q}\,\bar{S},$$

$$H = \bar{S}^\top\bar{Q}\,\bar{S} + \bar{R}.$$

If $H$ is positive definite then the minimum $z^*$ can be found by taking the partial derivative of $J$ with respect to $z$ and set it to zero

$$\frac{\partial\,J}{\partial\,z} = f + 2\,H\,z = 0$$

$$z^* = -\frac{1}{2}H^{-1}f = -\left(\bar{S}^\top\bar{Q}\,\bar{S} + \bar{R}\right)^{-1} \bar{S}^\top \bar{Q}\,\bar{T}\,x_0.$$

It can also be noted that using your definition of $\bar{Q}$ this control signal will be equivalent to finite horizon LQR. In order to guarantee stability of MPC you can add a roll-out policy. For example to make this problem really equivalent to infinite horizon LQR you could set the last diagonal term of $\bar{Q}$ equal to $X$, the solution of the discrete algebraic Riccati equation, instead of $Q$

$$X = A^\top X\,A - A^\top X\,B (R + B^\top X\,B)^{-1} B^\top X\,A + Q.$$

• You seems to know all control theory in the world? Right? :) – Daniel Mårtensson May 3 '18 at 17:08
• Hi again! To use a QP-solver, I need to us that solver on the form: $$J = \frac{1}{2}z^THz + f^Tz$$ In this case, I only need to dive $H$ and $f$ with 2? Why is the QP-form written as above? – Daniel Mårtensson May 13 '18 at 19:57
• @DanielMårtensson I suspect that the main reason it is often defined as such is because the partial derivative of $J$ with respect to $z$ then becomes $H\,z+f$, with the optimal solution $-H^{-1}\,f$. So no factor a half in the solution, which is probably considered to 'look' cleaner. – Kwin van der Veen May 14 '18 at 11:04
• So you mean that I can use $J = z^T H z + f^T z$ instead of the QP-formula above? – Daniel Mårtensson May 14 '18 at 16:54
• @DanielMårtensson In terms of problem formulation, yes. However QP in MATLAB/Octave does define $H$ such that the quadratic term of the cost function is multiplied by a half. – Kwin van der Veen May 14 '18 at 17:17