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*}$