In this example we consider the water tank system depicted on the right. Tank 1 has one input flow and one output flow. Also tank 2 has one input flow and one output flow. Tank 3 has two input flows and one output flow. The system dynamics are represented via the first equation below. As an output \(z\) we have a measurement of the level of tank 1 and of the level of tank 3.

\[ \begin{align*} x^+ &= Ax + Bu + v = \begin{bmatrix}1-\alpha_1 & 0 & 0 \\ 0 & 1-\alpha_2 & 0 \\ \alpha_1 & \alpha_2 & 1-\alpha_3\end{bmatrix}x + \begin{bmatrix} 0.5 \\ 0.5 \\ 0 \end{bmatrix}u + v \\ z &= H x + w + y = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix} x + w + y \end{align*} \]

The states of the system are \( x = [ x_1 \: x_2 \: x_3 ]^\top \) and as an input the flow \( u \) is given. There is a process noise \( v \) and a measurement noise \( w \), both are Gaussian Random Variables with mean \( 0 \) and variance \( Q \) and \(R \), i. e. \( v \sim \mathcal{N}(0,Q) \) and \( w \sim \mathcal{N}(0,R) \). The sparse signal \(y\), which is used to model sensor failures, distorts the measurement signal additionally.

The goal of this example is to show, that the standard Kalman Filter is not working that good anymore if sensor failures are present. There does not exist an analytic solution to the problem if the disturbance \(y\) is present. Using the robust Kalman Filter, i. e. replacing the standard measurement update step with an extended optimization problem, which is solved by FORCES Pro, the filter is robust against \(y\) and the estimated states are much more accurate compared to the standard Kalman Filter. To process the measurement data online, the optimization problem has to be solved in a sufficiently short amount of time.

Recall that the standard Kalman Filter, which would be applied if disturbance signal \( y \) were not present, consists of two steps: First a prediction step is made, where a predicted stated \( \hat{x}_\text{p}(k) \) is calculated based on the estimated state \( \hat{x}_\text{m}(k-1) \). Additionally, the predicted variance \( P_\text{p}(k) \) gets calculated in the prediction step. The measurement step returns the variance \( P_\text{m}(k) \) and the state esimate \( \hat{x}_\text{m}(k) \). This state estimate \( \hat{x}_\text{m}(k) \) is basically the solution of the optimization problem

$$ \begin{align*} \text{minimize} & \quad w^\top R^{-1} w + \left(x - \hat{x}_\text{p} \right)^\top P^{-1} \left(x - \hat{x}_\text{p} \right) \\ \text{subject to} & \quad z = Hx + w \end{align*} $$In this example, we assume that out of 100 measurements the sensors of tank 1 gand tank 3 gives each 5 bogus signals. In order to make the state estimator robust against the sensor failures \( y \), we solve the following convex optimization problem at every time instance

$$ \begin{align*} \text{minimize} & \quad w^\top R^{-1} w + \left(x - \hat{x}_\text{p} \right)^\top P^{-1} \left(x - \hat{x}_\text{p} \right) + \lambda \vert \vert y \vert \vert_\text{1} \\ \text{subject to} & \quad z = Hx + w + y \end{align*} $$In the optimization problem \( w \), \( x \) and \(y\) are optimization variables. The cost function of the optimization problem is extended with the \( \mathcal{l}_\text{1} \)-penaltiy which is non-quadratic. The value \( \lambda \geq 0 \) is a tuning parameter. For \( \lambda \) large enough, the solution of the optimization problem has \( y = 0 \) and therefore the estimates of the robust Kalman Filter coincides with the standard Kalman Filter solution. This optimization problem can be transformed as described in here. We transform this problem to the form required by FORCES Pro, which reads as

$$ \begin{align*} \text{minimize} & \quad \frac{1}{2} \tilde{z}^\top \tilde{H} \tilde{z} + f^\top \tilde{z} \\ \text{subject to} & \quad D\tilde{z} = c \\ & \quad A \tilde{z} \leq b \end{align*} $$where the optimization variable is given by \( \tilde{z} = [x^\top \: w^\top \: y^\top \: e^\top]^\top \). Please find below the MATLAB® code to generate the solver for the optimization problem with FORCES Pro. The covariance matrix \( P^{-1} \) is updated at every time step and therefore the problem can't be solved explicitly. In this problem three parameters need to be defined, which are \( H \), \( f \) - containing the predicted covariance and the predicated state - and \( c \) - contains the current measurement.

stages = MultistageProblem(1);

% Dimension

[ny nx] = size(H);

nw = ny;

ne = ny;

stages(1).dims.n = nx+nw+ny+ne; % number of stage variables

stages(1).dims.r = ny; % number of equality constraints

stages(1).dims.p = 2*ne; % number of polytopic constraints

% Ploytopic bounds

stages(1).ineq.p.A = [zeros(ny,nx), zeros(ny,nw), lambda*eye(ny), -eye(ne);...

zeros(ny,nx), zeros(ny,nw), -lambda*eye(ny), -eye(ne)];

stages(1).ineq.p.b = zeros(2*ne,1);

% Equality constraints

stages(1).eq.D = [H, eye(nw), eye(ny), zeros(ne)];

% Parameters

params(1) = newParam('H_i',1,'cost.H');

params(2) = newParam('f_i',1,'cost.f’);

params(3) = newParam('z_i',1,'eq.c');

% Output

outputs(1) = newOutput('x_hat_RKF',1,1:3);

% Code Generation

codeoptions = getOptions('Robust_KF');

generateCode(stages,params,codeoptions,outputs);

In the simulation the optimization problem has to be solved at every time instance. In the prediction step the state \( \hat{x}_\text{p} \) is calculated based on the estimation of the current state. Also the the variance is updated in every prediction step. In the measurement update step the estimated state \( \hat{x}_\text{m} \) is calculated based on the predicted state, its predicted variance and the current measurement \( z \) by the function `Robust_KF()` generated by FORCES Pro.

% Prediction Step

x_p_RKF = Ak(:,:,i-1)*x_hat_RKF(:,i-1)+B*u(i-1);

P_p_RKF(:,:,i) = Ak(:,:,i-1)*P_hat_RKF(:,:,i-1)*Ak(:,:,i-1)' + Q;

% Measurement Update Step - Optimization Problem

problem.H_i = [2*inv(P_p_RKF(:,:,i)),zeros(nx,nw+ny+ne);...

zeros(ny,nx),2*R_inv,zeros(ny,ny+ne);...

zeros(ny+ne,nx+nw+ny+ne)];

problem.f_i = [-2*(inv(P_p_RKF)*x_p_RKF);...

zeros(nw,1);...

zeros(ny,1);...

ones(ne,1)];

problem.z_i = z(:,i);

[solverout,exitflag,info] = Robust_KF(problem);

solve_time(1,i-1) = info.solvetime;

x_hat_RKF(:,i) = solverout.x_hat_RKF;

P_hat_RKF(:,:,i) = P_p_RKF(:,:,i);

end

In the plots below the estimated states are depicted. The estimates calculated via the robust Kalman Filter, in blue, are much more accurate then the standard approach. The peaks in the red line indicate sensor failures against which the standard Kalman Filter is not robust.

The impact on the RMS error magnitude of the robust Kalman Filter can be seen in the plots below. The magnitude of the robust Kalman Filter depicted in blue, is reduced by \( \sim 65 \% \) for state 1, \( \sim 12 \% \) for state 2, \( \sim 61 \% \) for state 3 (this values vary). Applying online optimization with FORCES Pro improves the quality of the state estimations significantly.

With FORCES Pro convex optimization can be embedded directly in signal processing algorithms that run online, with strict real-time deadlines, even at rates of tens of kilohertz. In this example the optimization problem is solved in \( \sim 0.1 ms \).