Time Propagation of State Vector Expectation and State Covariance Matrix of Linear Dynamical Systems – Intro to Kalman Filtering – Fusion of Engineering, Control, Coding, Machine Learning, and Science

In this post, we explain how the expected value of the state vector and state covariance matrix of linear dynamical systems are propagated in discrete-time. These equations, together with the equations describing the recursive least squares method which are derived in our previous post, are important for deriving the Kalman filter equations.

Linear Dynamical System Affected by the Process Noise (Disturbances)

We are considering the following state equation

(1) $\begin{align*}\mathbf{x}_{k}=A_{k-1}\mathbf{x}_{k-1}+B_{k-1}\mathbf{u}_{k-1}+\mathbf{w}_{k-1}\end{align*}$

where

$\mathbf{x}_{k}\in \mathbb{R}^{n}$ is the state vector
$\mathbf{u}_{k-1}\in \mathbb{R}^{m}$ is the control input vector
$\mathbf{w}_{k-1}\in \mathbb{R}^{n}$ is the disturbance or process noise vector. We assume that $\mathbf{w}_{k-1}$ is Gaussian, zero-mean, white noise with the covariance matrix given by $Q_{k}=E[\mathbf{w}_{k}\mathbf{w}_{k}^{T}]$ , where $E[\cdot]$ is the mathematical expectation operator.
$A_{k-1}\in \mathbb{R}^{n\times n}$ and $B\in \mathbb{R}^{n\times m}$ are state and input matrices.

A few comments are in order. If the system is time-invariant, then the matrices $A_{k}$ , $B_{k}$ , and $Q_{k}$ are constant, that is $A_{k}=A$ , $B_{k}=B$ , and $Q_{k}=Q$ . The process disturbance vector $\mathbf{w}_{k}$ can take into account unmodeled dynamics that is simply assumed as a stochastic disturbance, and/or stochastic disturbances affecting the state vector.

State Vector Expectation Propagation

Let us apply the expectation operator to the equation (1), as the result, we obtain

(2) $\begin{align*}E[\mathbf{x}_{k}]=E[A_{k-1}\mathbf{x}_{k-1}]+E[B_{k-1}\mathbf{u}_{k-1}]+E[\mathbf{w}_{k-1}] \end{align*}$

For notation brevity, we introduce the following notation

(3) $\begin{align*}E[\mathbf{x}_{k}]=\overline{\mathbf{x}}_{k}\end{align*}$

Taking into account that $E[\mathbf{w}_{k-1}] =0$ (the disturbance vector is zero mean), we obtain our final equation for state vector expectation propagation over discrete-time:

(4) $\begin{align*}\overline{\mathbf{x}}_{k}=A\overline{\mathbf{x}}_{k-1}+B_{k-1}\mathbf{u}_{k-1}\end{align*}$

State Covariance Matrix Propagation

We define the state covariance matrix as follows

(5) $\begin{align*}P_{k}=E[(\mathbf{x}_{k}-\overline{\mathbf{x}}_{k})(\mathbf{x}_{k}-\overline{\mathbf{x}}_{k})^{T}]\end{align*}$

Consequently, we first need to expand this expression

(6) $\begin{align*}(\mathbf{x}_{k}-\overline{\mathbf{x}}_{k})(\mathbf{x}_{k}-\overline{\mathbf{x}}_{k})^{T}\end{align*}$

and then compute the expectation of every term in that expression. By substituting the equations (1) and (4) in (10), we obtain

(7) $\begin{align*}& (\mathbf{x}_{k}-\overline{\mathbf{x}}_{k})(\mathbf{x}_{k}-\overline{\mathbf{x}}_{k})^{T} \\ & = (A_{k-1}(\mathbf{x}_{k-1}-\overline{\mathbf{x}}_{k-1})+\mathbf{w}_{k-1})(A_{k-1}(\mathbf{x}_{k-1}-\overline{\mathbf{x}}_{k-1})+\mathbf{w}_{k-1})^{T} \\& = A_{k-1}(\mathbf{x}_{k-1}-\overline{\mathbf{x}}_{k-1})(\mathbf{x}_{k-1}-\overline{\mathbf{x}}_{k-1})^{T}A_{k-1}^{T}+A_{k-1}(\mathbf{x}_{k-1}-\overline{\mathbf{x}}_{k-1})\mathbf{w}_{k-1}^{T} \\ & +\mathbf{w}_{k-1}(\mathbf{x}_{k-1}-\overline{\mathbf{x}}_{k-1})^{T}A_{k-1}^{T}+\mathbf{w}_{k-1}\mathbf{w}_{k-1}^{T}\end{align*}$

Since $(\mathbf{x}_{k}-\overline{\mathbf{x}}_{k})$ is uncorrelated with $\mathbf{w}_{k-1}$ , we have

(8) $\begin{align*}E[A_{k-1}(\mathbf{x}_{k-1}-\overline{\mathbf{x}}_{k-1})\mathbf{w}_{k-1}^{T}]=0 \\E[\mathbf{w}_{k-1}(\mathbf{x}_{k-1}-\overline{\mathbf{x}}_{k-1})^{T}A_{k-1}^{T}]=0\end{align*}$

Also, we have

(9) $\begin{align*}E[\mathbf{w}_{k-1}\mathbf{w}_{k-1}^{T}]=Q_{k-1}\end{align*}$

(10) $\begin{align*}P_{k-1}=E[(\mathbf{x}_{k-1}-\overline{\mathbf{x}}_{k-1})(\mathbf{x}_{k-1}-\overline{\mathbf{x}}_{k-1})^{T}]\end{align*}$

By using these expressions, and by taking the expectation of (7), we obtain the final equation for the propagation of the state covariance matrix

(11) $\begin{align*}P_{k}=A_{k-1}P_{k-1}A_{k-1}^{T}+Q_{k-1}\end{align*}$

Linear Dynamical System Affected by the Process Noise (Disturbances)

State Vector Expectation Propagation

State Covariance Matrix Propagation

admin

You might also like

Estimating the gravitational acceleration constant using Raspberry Pi, IR proximity sensors, and the least-squares method

Transient 3D Heat Conduction Simulation Using Finite Element Analysis in COMSOL Multiphysics

Computation of Eigenvalues and Eigenvectors in C++ by Using Eigen C++ Matrix Library