# Closed-loop Subspace Predictive Control Algorithm

This page briefly details the implementation of the controller. This discussion is limited to the steps taken by the code to arrive to a solution and does not cover the theoretical background of the controller. For rigorous explanation, please refer to the sources cited.

## System formulation

From [1], we use the lifted innovation form (equation 6)

$$
\begin{cases}
\hat{x}_k &= A^p \hat{x}_{k-p} + \mathcal{L} \bar{u}_{k-p, p} + \mathcal{K} \bar{e}_{k-p, p} \\
\bar{y}_{k, f} &= \Gamma \hat{x}_k + \mathcal{G} \bar{u}_{k, f} + \mathcal{H} \bar{e}_{k, f}
\end{cases}
$$

and the lifted predictor form (equation 7):

$$
\begin{cases}
\hat{x}_k &= \tilde{A}^p \hat{x}_{k-p} + \tilde{\mathcal{L}} \bar{u}_{k-p, p} + \tilde{\mathcal{K}} \bar{y}_{k-p, p} \\
\bar{y}_{k, f} &= \tilde{\Gamma} \hat{x}_k + \tilde{\mathcal{G}} \bar{u}_{k, f} + (I - \tilde{\mathcal{H}}) \bar{y}_{k, f} + \bar{e}_{k, f}
\end{cases}
$$ (system)

In the equations, $\bar{u}_{k-p, p}$ and $\bar{y}_{k-p, p}$ are stacked vectors of known past inputs and outputs, respectively.

We ignore the $\bar{e}$ terms and follow the assumption that $\tilde{A}^p = 0$ for $p$ sufficiently large; then combine the first equation of the predictor form with the second equation for the innovation form and multiply the first equation by $\Gamma$. This results in

$$
\begin{cases}
\Gamma \hat{x}_k &= \Gamma \tilde{\mathcal{L}} \bar{u}_{k-p, p} + \Gamma \tilde{\mathcal{K}} \bar{y}_{k-p, p} \\
\bar{y}_{k, f} &= \Gamma \hat{x}_k + \mathcal{G} \bar{u}_{k, f}
\end{cases}
$$

The matrices $\Gamma \tilde{\mathcal{L}}$, $\Gamma \tilde{\mathcal{K}}$ and $\mathcal{G}$ can be obtained from $\tilde{\Gamma} \tilde{\mathcal{L}}$, $\tilde{\Gamma} \tilde{\mathcal{K}}$ and $\tilde{\mathcal{G}}$, respectively, which in turn can be constructed from Markov parameters [1]. As a result, we now have an expression of the future output of the system as a function of past inputs and outputs and future inputs to the system. Assuming we know what we want the future outputs to be (i.e. following a reference), we can solve for the unknown future inputs.

## Calculating Markov Parameters

The Markov parameters needed to calculate $\tilde{\Gamma} \tilde{\mathcal{L}}$, $\tilde{\Gamma} \tilde{\mathcal{K}}$ and $\tilde{\mathcal{G}}$ are obtained using a square root formulation of a recursive least squares algorithm that includes directional forgetting ([2], section 4.2.2).

The directional forgetting is implemented using

$$
\begin{bmatrix}
0 & 0 \\
\sqrt{\alpha_{k}} \varphi_{k} & R_{k-1}
\end{bmatrix} Q_1 =
\begin{bmatrix}
0 & 0 \\
0 & \bar{R}_{k-1}
\end{bmatrix}
$$

where

$$
\sqrt{\alpha_k} = \sqrt{\frac{1-\lambda}{\lambda}}\frac{1}{\lVert R^{-1}_{k} \varphi_k \rVert}
$$

$R$ is the Cholesky factorisation of the covariance. $\bar{R}$ is the same matrix with directional forgetting applied. $\varphi$ contains the past inputs and outputs of the system. For the first update step, $\bar{R}_0$ is the identity matrix.

$Q_1$ is an arbitrary orthogonal transformation used to obtain the right-hand side matrix. Specifically, $Q_1$ consists of a series of Givens rotations.

Having obtained $\bar{R}_{k-1}$ from the directional forgetting step, we use the following to calculate covariance $R_k$ for the next update step, as well as $a$ and $G_k$, which are used to update the Markov parameters.

$$
\begin{bmatrix}
1 & \varphi^T_k \bar{R}_{k-1} \\
0 & \bar{R}_{k-1}
\end{bmatrix} Q_2 =
\begin{bmatrix}
a & 0 \\
G_k & R_k
\end{bmatrix}
$$

$$
\hat{\Theta}_k = \hat{\Theta}_{k-1} + \sqrt{\lambda} G_k a^{-1} \left(y_k^T - \varphi^T \hat{\Theta}_{k-a}\right)
$$

where $\hat{\Theta}$ are the Markov parameters and $Q_2$ is another orthogonal transformation using Givens rotations.

### Givens Rotations

Given two arbitrary elements $a$ and $b$ in a given row of a larger matrix, we want to zero one elements (say $b$). Using Givens rotations, this goes as follows ([4], Appendix B):

$$
\begin{bmatrix} a & b \end{bmatrix} \begin{bmatrix} c & -s \\ s & c \end{bmatrix} = \begin{bmatrix} r & 0 \end{bmatrix}
$$
$$r = \sqrt{a^2 + b^2}$$
$$c = \frac{a}{r}$$
$$s = \frac{b}{r}$$

Applying the Givens rotation to the matrix only affects the columns of elements $a$ and $b$. To avoid creating additional non-zero elements which need to be eliminated again, the Givens rotations are applied to the forgetting and update stages in a specific way.

For the directional forgetting, the elements in the first column must be eliminated ($b$). This is achieved by taking $a$ as the element on the main diagonal in the same row and going through the matrix front to back starting with the second row.

In the update step, the first row (except its first element) must be eliminated. This is done starting from the last column and working back to the second element in the row. Each time, $a$ is taken as the first element in the row. Since the first column is empty at the start, and gets filled up one element at a time from the bottom, no additional non-zero elements are created this way.

## Calculating System Matrices

Having obtained Markov parameters, We can construct the following matrices [1]:

$$
\tilde{\mathcal{G}} = \begin{bmatrix}
D & 0 & \dots & 0 \\
C\tilde{B} & D & \dots & 0 \\
\vdots & \vdots & \ddots & \vdots \\
C\tilde{A}^{f-2}\tilde{B} & C\tilde{A}^{f-3}\tilde{B} & \dots & D
\end{bmatrix}
$$

where

$$
\tilde{\Xi}^{(u_{k-i})} = \begin{cases}
D, &\text{if } i = 0 \\
C\tilde{A}^{i-1}\tilde{B}, &\text{if } i > 0
\end{cases}
$$

$$
\tilde{\Gamma}\tilde{\mathcal{L}} = \begin{bmatrix}
\tilde{\Xi}^{(u_{k-p})} & \tilde{\Xi}^{(u_{k-p+1})} & \dots & \tilde{\Xi}^{(u_{k-p+f-1})} & \dots & \tilde{\Xi}^{(u_{k-1})} \\
0 & \tilde{\Xi}^{(u_{k-p})} & \dots & \tilde{\Xi}^{(u_{k-p+f-2})} & \dots & \tilde{\Xi}^{(u_{k-2})} \\
\vdots & & & \vdots & \ddots & \vdots \\
0 & & \dots & \tilde{\Xi}^{(u_{k-p})} & \dots & \tilde{\Xi}^{(u_{k-f})}
\end{bmatrix}
$$

$$
\tilde{\Gamma}\tilde{\mathcal{K}} = \begin{bmatrix}
\tilde{\Xi}^{(y_{k-p})} & \tilde{\Xi}^{(y_{k-p+1})} & \dots & \tilde{\Xi}^{(y_{k-p+f-1})} & \dots & \tilde{\Xi}^{(y_{k-1})} \\
0 & \tilde{\Xi}^{(y_{k-p})} & \dots & \tilde{\Xi}^{(y_{k-p+f-2})} & \dots & \tilde{\Xi}^{(y_{k-2})} \\
\vdots & & & \vdots & \ddots & \vdots \\
0 & & \dots & \tilde{\Xi}^{(y_{k-p})} & \dots & \tilde{\Xi}^{(y_{k-f})}
\end{bmatrix}
$$

In addition, the matrix $\tilde{H}$ is needed to convert $\tilde{\Gamma} \tilde{\mathcal{L}}$, $\tilde{\Gamma} \tilde{\mathcal{K}}$ and $\tilde{\mathcal{G}}$ to $\Gamma \tilde{\mathcal{L}}$, $\Gamma \tilde{\mathcal{K}}$ and $\mathcal{G}$, respectively.

$$
\tilde{\mathcal{H}} = \begin{bmatrix}
I & 0 & \dots & 0 \\
-CK & I & & \vdots \\
\vdots & \vdots & \ddots & 0 \\
- C\tilde{A}^{f-2}K & -C\tilde{A}^{f-3}K & \dots & I
\end{bmatrix}
$$

with

$$
\tilde{\Xi}^{(y_{k-i})} = C\tilde{A}^{i-1}K
$$


We now obtain $\Gamma \tilde{\mathcal{L}}$ like so:

$$
\Gamma \tilde{\mathcal{L}} = \tilde{H}^{-1} \tilde{\Gamma} \tilde{\mathcal{L}}
$$

$\Gamma \tilde{\mathcal{K}}$ and $\mathcal{G}$ can be obtained in the same way.

## Calculating Future Inputs

The future inputs are obtained as the optimal of the following cost function ([3], section 5.2.2):

$$
J = \left( \bar{y}_f - r_f \right)^T Q_a \left( \bar{y}_f - r_f \right) + u^T_f R_a u_f + \Delta u^T_f R^\Delta_a \Delta u_f
$$

where $\bar{y}_f$ is the future output as given by equation {eq}`system`, $r_f$ if the future reference and $Q_a$, $R_a$ and $R^\Delta_a$ are diagonal matrices of the form $k\cdot I$ with $I$ being the identity matrix. Furthermore

$$
\Delta u_f = S_\Delta u_f - S_v u_p
$$

and

$$
S_\Delta \in \mathbb{R}^{f\times f} = \begin{bmatrix}
1 & 0 & 0 & \dots & 0 \\
-1 & 1 & 0 & \dots & 0 \\
0 & -1 & 1 & & \vdots \\
\vdots & & \ddots & \ddots & 0 \\
0 & \dots & 0 & -1 & 1
\end{bmatrix}
$$
$$
S_v \in \mathbb{R}^{f\times p} = \begin{bmatrix}
0 & \dots & 0 & 1 \\
0 & \dots & 0 & 0 \\
\vdots & & \vdots & \vdots \\
0 & \dots & 0 & 0
\end{bmatrix}
$$

The cost function balances reference error, input magnitude as well as input change.

The cost function is rewritten in the form of a quadratic program.

$$
J = u^T_f P u_f + q^T u_f
$$

with

$$
P = \mathcal{G}^T Q_a \mathcal{G} + S^T_\Delta R^\Delta_a S_\Delta + R_a
$$

$$
q^T = 2\left(u^T_p \Gamma \tilde{\mathcal{L}} Q_a \mathcal{G} + u^T_p \Gamma \tilde{\mathcal{K}} Q_a \mathcal{G} - r^T_f Q \mathcal{G} - u^T_p S^T_v R^\Delta_a S_\Delta \right)
$$


## References

[1] I. Houtzager, J.W. van Wingerden and M. Verhaegen, "Recursive Predictor-Based Subspace Identification With Application to the Real-Time Closed-Loop Tracking of Flutter," in IEEE Transactions on Control Systems Technology, vol. 20, no. 4, pp. 934-949, July 2012, doi: [10.1109/TCST.2011.2157694](https://doi.org/10.1109/TCST.2011.2157694)

[2] G. J. Van Der Veen, ‘Identification of wind energy systems’, Doctoral Thesis, Delft University of Technology, Delft, 2013. Available: http://resolver.tudelft.nl/uuid:6c26c923-1518-4e53-bd22-f66c6b49ae7c

[3] R. Hallouzi, ‘Multiple-model based diagnosis for adaptive fault-tolerant control’, Delft University of Technology, Delft, 2008. Available: http://resolver.tudelft.nl/uuid:ecb17462-4c8c-484a-af33-e41d57773ceb

[4] R. Hallouzi and M. Verhaegen, ‘Reconfigurable Fault Tolerant Control of a Boeing 747 Using Subspace Predictive Control’, in AIAA Guidance, Navigation and Control Conference and Exhibit, in Guidance, Navigation, and Control and Co-located Conferences. , American Institute of Aeronautics and Astronautics, 2007. doi: [10.2514/6.2007-6665](https://arc.aiaa.org/doi/abs/10.2514/6.2007-6665).