# Optimal Control In Tracking Problem

I'm having a hard time getting to a solution to this... I have a nonlinear control system tracking a time-variant reference in which I'm going to implement a Gain-Scheduled controller whose gains are obtained through Optimal Control Laws.

Through linearization of my system I have:

\begin{align} \dot{x}&=Ax+Bu\\ y &=Cx \end{align}

Just in case it helps: $A \in \mathbb R^{12\times12}$, $B \in \mathbb R^{12\times4}$ and $C \in \mathbb R^{4\times12}$ from where it is possible to notice that $x \in \mathbb R^{12}$, $u \in \mathbb R^{4}$ and $y \in \mathbb R^{4}$.

Now, through state and output feedback, I want to track a reference $r \in \mathbb R^{4}$ so that:

\begin{align} \dot{x}&=Ax+Bu\\ y &=Cx\\ u &=-Kx+F(r-y) \end{align}

For this architecture, I considered my Cost Functional as:

$$J=\int \left(x^{T}Qx+u^{T}Ru\right)dt$$

While having $Q \in \mathbb R^{12\times12}$ and $R \in \mathbb R^{4\times4}$, in my case, both positive definite.

And solved the Algebraic Riccati's Equation as: $A^{T}P+PA-PBR^{-1}B^{T}P+Q=0$

And then $S=R^{-1}B^{T}P$ is my complete gain matrix.

However, $F \in \mathbb R^{4\times4}$ is a selection of 4 columns from $S$. Then, in $S$ they were substituted by $0$ columns to constitute altogether the state feedback gain matrix $K\in \mathbb R^{4\times12}$.

The system was successfully stabilized, however I also expected to see $y\to r$ in an infinite time horizon, but when I tried to implement this, a steady state error appeared and I was unable to correct it without and integrator in the forward path of the control system. At the same time, if I try to insert the integrator without recalculating the gains, the system becomes unstable. Now I have this:

\begin{align} \dot{x}&=Ax+Bu\\ \dot{e}&=r-y\\ y &=Cx\\ u &=-Kx+Fe \end{align}

Which defines an extended State Space Model:

\begin{align} \left(\begin{array}{c} \dot{x}\\ \dot{e} \end{array}\right)&= \left(\begin{array}{cc} A & 0_{12\times4}\\ -C & 0_{4\times4}\end{array}\right)\left(\begin{array}{c} x\\ e\end{array}\right) +\left(\begin{array}{c} B\\ 0_{4\times4}\end{array}\right)u+\left(\begin{array}{c} 0_{12\times4}\\ I_{4\times4}\end{array}\right)r\\ y&=\left(\begin{array}{c} C & 0_{4\times4} \end{array}\right)\left(\begin{array}{c} x\\ e\end{array}\right) \end{align}

Let $\mathcal A$, $\mathcal B$ and $\mathcal C$ be the Extended Space State matrices defined above and $z=\left(\begin{array}{c}x \\ e\end{array}\right)$

If I say that $\mathcal R = R$ and define a new $\mathcal Q \in \mathbb R^{16x16}$ as:

$$\mathcal Q = \left(\begin{array}{cc} Q_{x} & 0_{12\times4} \\ 0_{4\times12} & Q_{e}\end{array}\right)$$

Should I work on this cost functional:

$$J=\int \left(z^{T}\mathcal Q z+u^{T}\mathcal R u\right)dt=\int \left(x^{T}Q_{x}x+e^{T}Q_{e}e+u^{T}\mathcal R u\right)dt$$

and have the following ARE: $\mathcal A^{T}P+P\mathcal A-P\mathcal B \mathcal R^{-1} \mathcal B^{T}P+\mathcal Q=0$

Or this one:

$$J=\int \left(e^{T}Q_{e}e+u^{T}\mathcal R u\right)dt$$

with the following ARE: $\mathcal A^{T}P+P\mathcal A-P\mathcal B \mathcal R^{-1} \mathcal B^{T}P+\mathcal C^{T}Q_{e}C=0$ ?

It seems to me that the last one is what I should use, in order to try and make $e \to 0$. However, MATLAB says it's unable to solve that ARE, once its Hamiltonian Spectrum is too close to the imaginary axis.

Ah, and by the way, both $\left(A,B\right)$ and $\left(\mathcal A, \mathcal B\right)$ are controllable.

One might be able to get your second approach, with integrator, to work, however I would think adding feed-forward might be an easier solution. Namely when you reach your reference goal your system has to satisfy $C\,x_r + D\,u_r = r$ and $\dot{x}_r = A\, x_r + B\,u_r = 0$, or when writing it as one system of linear equations

$$\begin{bmatrix} A & B \\ C & D \end{bmatrix} \begin{bmatrix} x_r \\ u_r \end{bmatrix} = \begin{bmatrix} 0_{12\times1} \\ r \end{bmatrix}.$$

Assuming that that matrix is invertible, then solving for $x_r$ and $u_r$ yields

$$\begin{bmatrix} x_r \\ u_r \end{bmatrix} = \begin{bmatrix} A & B \\ C & D \end{bmatrix}^{-1} \begin{bmatrix} 0_{12\times1} \\ r \end{bmatrix}.$$

By defining new state space coordinates and input $\hat{x} = x - x_r$ and $\hat{u} = u - u_r$ respectively, then its dynamics can be written as

$$\dot{\hat{x}} = \dot{x} - \dot{x}_r = A\, x + B\, u = A (\hat{x} + x_r) + B (\hat{u} + u_r) = A\, \hat{x} + B\, \hat{u} + \underbrace{A\, x_r + B\,u_r}_{0}.$$

So $x$ can be driven to $x_r$ ($\hat{x} = 0$) by using the control law $\hat{u} = -K\, \hat{x}$ (for all $K$ such that $A-B\,K$ has all its eigenvalues in the open left half plane). This implies that the actual input $u$ should be equal to

$$u = \hat{u} + u_r = -K (x - x_r) + u_r.$$

When substituting in the solution for $x_r$ and $u_r$ yields

$$u = -K\,x + \begin{bmatrix} K & I_{4\times4} \end{bmatrix} \begin{bmatrix} A & B \\ C & D \end{bmatrix}^{-1} \begin{bmatrix} 0_{12\times4} \\ I_{4\times4} \end{bmatrix} r.$$

Since all these matrices are constant, then by defining a new matrix as

$$F = \begin{bmatrix} K & I_{4\times4} \end{bmatrix} \begin{bmatrix} A & B \\ C & D \end{bmatrix}^{-1} \begin{bmatrix} 0_{12\times4} \\ I_{4\times4} \end{bmatrix}$$

you can see that with your first attempt you where not that far off from this feed-forward method in terms of control law.

It can be noted that this method assumes that $r$ is constant, however when the reference changes over time, then your tracking performance might not be great. And after some playing with this myself it even seemed that choosing a $K$, such that the real parts of the poles of $A-B\,K$ are more negative, negatively affected the tracking performance a high frequencies.

It might also be worth noting that one can define a new state space model which captures the resulting reference-output dynamics as follows:

$$\left\{ \begin{array}{l} \dot{x} = \underbrace{(A - B\,K)}_{A_r}\,x + \underbrace{B\,F}_{B_r}\,r \\ y = \underbrace{(C - D\,K)}_{C_r}\,x + \underbrace{D\,F}_{D_r}\,r \end{array}\right.$$

I did a quick search online and found this document which basically contains the same derivation of the feed-forward method. It also contains the integral method, however it stops once they defined the extended state space and only suggested a control law $u = K\,x - K_I\,e$, but not how to find such $K$ and $K_I$.

I was determined to also find a solution to the integrator approach myself. But ran into difficulties using only the state space extension of $\dot{e} = r - y$. After some trial and error I decided on an extend state space of

$$z = \begin{bmatrix} x \\ e \\ r \\ u \end{bmatrix}$$

By defining a new input to this system as $v = \dot{u}$ then the dynamics can be formulate as

$$\dot{z} = \underbrace{ \begin{bmatrix} A & 0 & 0 & B \\ C & 0 & -I & D \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix}}_{\hat{A}} z + \underbrace{ \begin{bmatrix} 0 \\ 0 \\ 0 \\ I \end{bmatrix}}_{\hat{B}} v.$$

As time goes to infinity one would like $y-r$, so $\dot{e}$, to go to zero, however one would also like the system to be internally stable, this would happen if $\dot{x}$ and $\dot{u}$ would go to zero as well. So a cost function for an optimal controller could be defined as

$$\begin{array}{rl} J = & \int \left(\dot{e}^\top Q_e\, \dot{e} + \dot{x}^\top Q_x\, \dot{x} + \dot{u}^\top Q_u\, \dot{u}\right) dt \\ = & \int \dot{z}^\top \underbrace{ \begin{bmatrix} Q_x & 0 & 0 & 0 \\ 0 & Q_e & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & Q_u \end{bmatrix}}_{\hat{Q}} \dot{z}\, dt \end{array}$$

where $Q_e$, $Q_x$ and $Q_u$ are positive definite matrices. Substituting in the expression for $\dot{z}$ gives

$$J = \int \left(z^\top Q\, z + v^\top R\, v + 2\, z^\top N\, v\right) dt,$$

$$Q = \hat{A}^\top \hat{Q}\, \hat{A},$$

$$R = \hat{B}^\top \hat{Q}\, \hat{B} = Q_u,$$

$$N = \hat{A}^\top \hat{Q}\, \hat{B} = 0.$$

This just in the standard LQR form. However I had some trouble solving this consistently, because often due to numerically errors $Q$ was not positive semi-definite and $(\hat{A},\hat{B})$ has a boundary stable mode that is not controllable. One way I was able to get this to work with some success was by adding an identity matrix times a very small positive value $\epsilon$ to $Q$ to increase all its eigenvalues by $\epsilon$ and by adding very weak damping to $r$ by setting the third matrix on the diagonal of $\hat{A}$ equal to an identity matrix times a very small negative value. If you did manage to find a controller $v = -K\,z$ with

$$K = \begin{bmatrix} -K_x & -K_e & -K_r & -K_u \end{bmatrix},$$

then the closed-loop could also be written as

$$\begin{bmatrix} \dot{x} \\ \dot{e} \\ \dot{u} \end{bmatrix} = \begin{bmatrix} A & 0 & B \\ C & 0 & D \\ K_x & K_e & K_u \end{bmatrix} \begin{bmatrix} x \\ e \\ u \end{bmatrix} + \begin{bmatrix} 0 \\ -I \\ K_r \end{bmatrix} r,$$

$$y = \begin{bmatrix} C & 0 & D \end{bmatrix} \begin{bmatrix} x \\ e \\ u \end{bmatrix}.$$

When looking at the relevant CARE I believe it will always be the case that $K_e = 0$. If this is true then the closed-loop can be reduced even further, by removing the state $e$. Doing this yields

$$\begin{bmatrix} \dot{x} \\ \dot{u} \end{bmatrix} = \begin{bmatrix} A & B \\ K_x & K_u \end{bmatrix} \begin{bmatrix} x \\ u \end{bmatrix} + \begin{bmatrix} 0 \\ K_r \end{bmatrix} r,$$

$$y = \begin{bmatrix} C & D \end{bmatrix} \begin{bmatrix} x \\ u \end{bmatrix}.$$

After some more searching I also came across what is called Linear-Quadratic-Integral. This indeed does defines tries to find an input of the form $u = -K\,x - F\,e$ which tries to minimize the following cost function

$$J = \int \left(\begin{bmatrix}x \\ e\end{bmatrix}^\top Q\, \begin{bmatrix}x \\ e\end{bmatrix} + u^\top R\, u + 2\,\begin{bmatrix}x \\ e\end{bmatrix}^\top N\,u\right) dt.$$

According to the function definition of lqi in MATLAB its corresponding ARE it does indeed tries to solve the same as your first ARE

$$\begin{bmatrix}A & 0 \\ -C & 0\end{bmatrix}^\top P + P \begin{bmatrix}A & 0 \\ -C & 0\end{bmatrix} - \left(P \begin{bmatrix}B & -D\end{bmatrix} + N\right) R^{-1} \left( \begin{bmatrix}B & -D\end{bmatrix}^\top P + N^\top\right) + Q = 0.$$

After some testing I found that this method seems to be identical to mine when you choose $Q$ for lqi the same as your $\mathcal{Q}$, so placing $Q_x$ and $Q_e$ on its diagonal, $R = Q_u$ and $N = 0$. This was somewhat unexpected since I used those same matrices to minimize $\dot{x}$, $\dot{e}$ and $\dot{u}$ instead of $x$, $e$ and $u$. The advantage of this method over mine would be that it does not seem to run into the same troubles as mine did and the corresponding ARE it tries to solve is of smaller dimension which should also make it a little faster to solve.

• That is indeed a good alternative! Thanks for the attention and the well formulated answer! May 30, 2017 at 18:10
• @bertozzijr I do believe that you can only design such a reference tracking controller for "constant" references, because otherwise your dynamics I believe would be time varying, in which is not LTI. Under this assumption only you only know that $\dot{e}$ and $\dot{x}$ should go to zero as $t$ goes to $\infty$, so maybe you could define an integral with quadratic term in those state variables as cost function and derive an ARE from that. May 30, 2017 at 18:31
• @bertozzijr I have updated my answer with my own solution to the integrator approach. Jun 1, 2017 at 16:30