LMIs in Control/Applications/An LMI for the Kalman Filter

LMIs in Control/Applications/An LMI for the Kalman Filter

This is a An LMI for the Kalman Filter. The Kalman Filter is one of the most widely used state-estimation techniques. It has applications in multiple aspects of navigation (inertial, terrain-aided, stellar.)

The System

Continuous Time:

${\begin{aligned}{\dot {x}}(t)&=Ax(t)+Bu(t)+w(t),\\y(t)&=Cx(t)+v(t)\\\end{aligned}}$

The process and sensor noises are given by $w(t)$ and $v(t)$ respectively.

Discrete Time:

${\begin{aligned}x_{k+1}&=Ax_{k}+w_{k}\\y_{k}&=Cx_{k}+v_{k}\end{aligned}}$

The process and sensor noises are given by $w_{k}$ and $v_{k}$ respectively.

The Data

The data required for the Kalman Filter include a model of the system that the states are trying to be output and a measurement that is the output of the system dynamics being estimated.

The Filter

The Filter and Estimator equations can be written as:

Continuous Time

${\begin{aligned}{\dot {\hat {x}}}(t)&=A{\hat {x}}(t)+Bu(t)+L(y(t)-{\hat {y}}(t))\\\end{aligned}}$

Discrete Time

${\begin{aligned}{\hat {x}}_{k+1}&=A{\hat {x}}_{k}+L(C{\hat {x}}_{k}-y_{k})\\\end{aligned}}$

The Error

The error dynamics evolve according to the following expression

Continuous Time

${\begin{aligned}{\dot {e}}(t)&=(A+LC)e(t)+w(t)+Lv(t)\\\end{aligned}}$

Discrete Time

${\begin{aligned}e_{k+1}&=(A+LC)e_{k}+w_{k}+Lv_{k}\\\end{aligned}}$

The Optimization Problem

The Kalman Filtering (or LQE) problem is a Dual to the LQR problem. Replace the matrices $(A,B,Q,R,K)$ from LQR with $(A^{T},C^{T},V_{1},V_{2},L^{T}).$

The Kalman Filter chooses $L$ to minimize the cost $J=E[e^{T}e].$ This cost can be thought of as the covariance of the state error between the actual and estimated state. When the state error covariance is low the filter has converged and the estimate is good.

The Luenberger or Kalman gain can be computed from $L=\Sigma C^{T}V_{2}^{-1}$

The process and measurement noise covariances for the Kalman filter are given by

${\begin{aligned}V_{1}&=E[w(t)w(t)^{T}]{\text{ aka: }}Q\\V_{2}&=E[v(t)v(t)^{T}]{\text{ aka: }}R\\\end{aligned}}$

The matrix $\Sigma$ satisfies the following equality

$A\Sigma +\Sigma A^{T}+V_{1}=\Sigma C^{T}V_{2}^{-1}C\Sigma$

We also cover the discrete Kalman Filter formulation which is more useful for real-life computer implementations.

The discrete Kalman filter chooses the gain $L=A\Sigma C^{T}(C\Sigma C^{T}+V)^{-1}$ where the PSDs of the process and sensor noises are given by

${\begin{aligned}W&=E[w_{k}w_{k}^{T}]{\text{ aka: }}Q\\V&=E[v_{k}v_{k}^{T}]{\text{ aka: }}R\\\end{aligned}}$

The steady-state covariance of the error in the estimated state is given by $\Sigma$ and satisfies the following Riccati equation.

${\begin{aligned}\Sigma =A\Sigma A^{T}+W-A\Sigma C^{T}(C\Sigma C^{T}+V)^{-1}C\Sigma A^{T}\end{aligned}}$

• Objective: State Estimate Error Covariance
• Variables: Observer Gains
• Constraints: Dynamics of System to be Estimated

The LMI: H2-Optimal Control Full-State Feedback to LQR to Kalman Filter

The Kalman Filter is a dual to the LQR problem which has been shown to be equivalent to a special case of H2-static state feedback.

Start with the H2-Optimal Control Full-State Feedback.

The following are equivalent

${\begin{aligned}1.||S(K,P)||_{H2}&<\gamma \\\end{aligned}}$

${\begin{aligned}2.K&=ZX^{-1}{\text{ for some }}Z{\text{ and }}X>0{\text{ where}}\\\end{aligned}}$

${\begin{aligned}{\begin{bmatrix}A&B_{2}\end{bmatrix}}{\begin{bmatrix}X\\Z\\\end{bmatrix}}+{\begin{bmatrix}X&Z^{T}\end{bmatrix}}{\begin{bmatrix}A^{T}\\B_{2}^{T}\\\end{bmatrix}}+B_{1}B_{1}^{T}<0\\{\begin{bmatrix}X&(C_{1}X+D_{12}Z)^{T}\\C_{1}X+D_{12}Z&W\end{bmatrix}}>0\\{\text{Trace}}(W)<\gamma ^{2}\end{aligned}}$

To solve the LQR problem using H2 optimal state-feedback control the following variable substitutions are required.

${\begin{aligned}C_{1}={\begin{bmatrix}Q^{1/2}\\0\\\end{bmatrix}},D_{12}={\begin{bmatrix}R^{1/2}\\0\\\end{bmatrix}},D_{11}=0,\\B_{2}=B,B_{1}=I\end{aligned}}$

Then

${\begin{aligned}S(P,K)={\begin{bmatrix}A+B_{2}K&B_{1}\\C_{1}+D_{12}K&D_{11}\\\end{bmatrix}}={\begin{bmatrix}A+BK&I\\Q^{1/2}&0\\K&0\\\end{bmatrix}}\end{aligned}}$

This results in the following LMI.

${\begin{aligned}{\begin{bmatrix}A&B_{2}\end{bmatrix}}{\begin{bmatrix}X\\Z\\\end{bmatrix}}+{\begin{bmatrix}X&Z^{T}\end{bmatrix}}{\begin{bmatrix}A^{T}\\B_{2}^{T}\\\end{bmatrix}}+B_{1}B_{1}^{T}<0\\{\begin{bmatrix}X&({\begin{bmatrix}Q^{1/2}\\0\\\end{bmatrix}}X+{\begin{bmatrix}R^{1/2}\\0\\\end{bmatrix}}Z)^{T}\\{\begin{bmatrix}Q^{1/2}\\0\\\end{bmatrix}}+{\begin{bmatrix}R^{1/2}\\0\\\end{bmatrix}}Z&W\end{bmatrix}}>0\\TraceW<\gamma \\\end{aligned}}$

To solve the Kalman Filtering problem using the LQR LMI replace $A,B,Q,R,K$ with $A^{T},C^{T},V_{1},V_{2},$ and $L^{T}.$ This results in the following LMI.

${\begin{aligned}{\begin{bmatrix}A^{T}&C^{T}\end{bmatrix}}{\begin{bmatrix}X\\Z\\\end{bmatrix}}+{\begin{bmatrix}X&Z^{T}\end{bmatrix}}{\begin{bmatrix}A\\C\\\end{bmatrix}}+I<0\\{\begin{bmatrix}X&({\begin{bmatrix}V_{1}^{1/2}\\0\\\end{bmatrix}}X+{\begin{bmatrix}V_{2}^{1/2}\\0\\\end{bmatrix}}Z)^{T}\\{\begin{bmatrix}V_{1}^{1/2}\\0\\\end{bmatrix}}+{\begin{bmatrix}V_{2}^{1/2}\\0\\\end{bmatrix}}Z&W\end{bmatrix}}>0\\TraceW<\gamma \\\end{aligned}}$

The discrete-time Kalman Filtering LMI is saved for another page as it requires derivation of the Discrete-Time LQR LMI problem which was not covered in class.

Conclusion:

The LMI for the Kalman Filter allows us to calculate the optimal gain for state estimation. It is shown that it can be found as a special case of the H2-optimal state feedback with the appropriate substitution of matrices. The LMI gives us a different way of computing the optimal Kalman gain.

Implementation

A link to CodeOcean or other online implementation of the LMI

Related LMIs

Links to other closely-related LMIs

• H2OptimalObserver

External Links

A list of references documenting and validating the LMI.

• LMI Methods in Optimal and Robust Control - A course on LMIs in Control by Matthew Peet.
• LMI Properties and Applications in Systems, Stability, and Control Theory - A List of LMIs by Ryan Caverly and James Forbes
• LMIs in Systems and Control Theory - A downloadable book on LMIs by Stephen Boyd
• [1] - Applied Optimal Estimation, Arthur Gelb (Kalman Filtering and Optimal Estimation Classic)
• [2] - All Source Position, Navigation and Timing. New textbook that has a good development and description of the Kalman Filter and some of its very practical uses and implementations by Boeing Senior Technical Fellow Ken Li.