﻿ Covariance Update Equation

# Covariance Update Equation

The Covariance Update Equation is given by:

$\boldsymbol{P}_{n,n} = \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \boldsymbol{P}_{n,n-1} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} + \boldsymbol{K}_{n}\boldsymbol{R}_{n}\boldsymbol{K}_{n}^{T}$
where:
 $$\boldsymbol{P}_{n,n}$$ is the covariance matrix of the current state estimation $$\boldsymbol{P}_{n,n-1}$$ is the prior estimate covariance matrix of the current state (predicted at the previous state) $$\boldsymbol{K}_{n}$$ is the Kalman Gain $$\boldsymbol{H}$$ is the observation matrix $$\boldsymbol{R}_{n}$$ is the measurement noise covariance matrix $$\boldsymbol{I}$$ is an Identity Matrix (the $$n \times n$$ square matrix with ones on the main diagonal and zeros elsewhere)

## Covariance Update Equation Derivation

This section includes the Covariance Update Equation derivation. Some of you may find it too detailed, but on the other hand, it will help others to understand better.

For the derivation, I use the following four equations:

Eq. Num. Equation Notes
1 $$\boldsymbol{\hat{x}}_{n,n} = \boldsymbol{\hat{x}}_{n,n-1} + \boldsymbol{K}_{n} ( \boldsymbol{z}_{n} - \boldsymbol{H \hat{x}}_{n,n-1} )$$ State Update Equation
2 $$\boldsymbol{z}_{n} = \boldsymbol{Hx}_{n} + \boldsymbol{v}_{n}$$ Measurement Equation
3 $$\boldsymbol{P}_{n,n} = E\left( \boldsymbol{e}_{n}\boldsymbol{e}_{n}^{T} \right) = E\left( \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n} \right)^{T} \right)$$ Estimate Covariance
4 $$\boldsymbol{R}_{n} = E\left( \boldsymbol{v}_{n}\boldsymbol{v}_{n}^{T} \right)$$ Measurement Covariance

We derive the Current Estimate Covariance ( $$\boldsymbol{P}_{n,n}$$ ) as a function of the Kalman Gain $$\boldsymbol{K}_{n}$$.

Notes
$$\boldsymbol{\hat{x}}_{n,n} = \boldsymbol{\hat{x}}_{n,n-1} + \boldsymbol{K}_{n} ( \boldsymbol{z}_{n} - \boldsymbol{H \hat{x}}_{n,n-1} )$$ State Update Equation
$$\boldsymbol{\hat{x}}_{n,n} = \boldsymbol{\hat{x}}_{n,n-1} + \boldsymbol{K}_{n} ( \boldsymbol{Hx}_{n} + \boldsymbol{v}_{n} - \boldsymbol{H \hat{x}}_{n,n-1} )$$ Plug the Measurement Equation into the State Update Equation
$$\boldsymbol{e}_{n} = \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n}$$ Estimate error
$$\boldsymbol{e}_{n} = \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} - \boldsymbol{K}_{n} \left(\boldsymbol{Hx}_{n} + \boldsymbol{v}_{n} - \boldsymbol{H \hat{x}}_{n,n-1} \right)$$ Plug $$\boldsymbol{\hat{x}}_{n,n}$$
$$\boldsymbol{e}_{n} = \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} - \boldsymbol{K}_{n}\boldsymbol{Hx}_{n} - \boldsymbol{K}_{n}\boldsymbol{v}_{n} + \boldsymbol{K}_{n}\boldsymbol{H \hat{x}}_{n,n-1}$$ Expand
$$\boldsymbol{e}_{n} = \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} - \boldsymbol{K}_{n}\boldsymbol{H}\left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n}$$ Localize $$(\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1})$$
$$\boldsymbol{e}_{n} = \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n}$$
$$\boldsymbol{P}_{n,n} = E\left( \boldsymbol{e}_{n}\boldsymbol{e}_{n}^{T} \right) = E\left( \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n} \right)^{T} \right)$$ Estimate Covariance
$$\boldsymbol{P}_{n,n} = E\left( \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right) \times \\ \times \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} \right)$$ Plug $$\boldsymbol{e}_{n}$$
$$\boldsymbol{P}_{n,n} = E\left( \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right) \times \\ \times \left( \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \right)^{T} - \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n}\right) ^{T} \right) \right)$$ Expand
$$\boldsymbol{P}_{n,n} = E\left(\left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right) \times \\ \times \left( \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} - \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n}\right)^{T} \right) \right)$$ Apply the matrix transpose property: $$(\boldsymbol{AB})^{T} = \boldsymbol{B}^{T}\boldsymbol{A}^{T}$$
$$\boldsymbol{P}_{n,n} = E \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} - \\ - \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} - \\ - \boldsymbol{K}_{n}\boldsymbol{v}_{n} \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} + \\ + \boldsymbol{K}_{n}\boldsymbol{v}_{n} \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} \right)$$ Expand
$$\boldsymbol{P}_{n,n} = E \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} \right) - \\ - \color{red}{E \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} \right)} - \\ - \color{red}{E \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} \right)} + \\ + E \left( \color{blue}{\boldsymbol{K}_{n}\boldsymbol{v}_{n} \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} }\right)$$ Apply the rule $$E(X \pm Y) = E(X) \pm E(Y)$$
$$\color{red}{E \left( \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \right)^{T} \right) = 0}$$
$$\color{red}{E \left( \boldsymbol{K}_{n}\boldsymbol{v}_{n} \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} \right) = 0}$$
$$(\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1})$$ is the error of the prior estimate. It is uncorrelated with the current measurement noise $$\boldsymbol{v}_{n}$$. The expectation value of the product of two uncorrelated variables is zero.
$$\boldsymbol{P}_{n,n} = E \left( \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left( \boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \left( \boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} \right) + \\ + E \left( \color{blue}{\boldsymbol{K}_{n}\boldsymbol{v}_{n}\boldsymbol{v}_{n}^{T}\boldsymbol{K}_{n}^{T}}\right)$$ Apply the matrix transpose property: $$(\boldsymbol{AB})^{T} = \boldsymbol{B}^{T}\boldsymbol{A}^{T}$$
$$\boldsymbol{P}_{n,n} = \boldsymbol{ \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)} \color{green}{E \left( \left(\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left(\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \right)} \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} + \\ + \boldsymbol{K}_{n} \color{blue}{ E \left( \boldsymbol{v}_{n}\boldsymbol{v}_{n}^{T} \right) } \boldsymbol{ K}_{n}^{T}$$ Apply the rule $$E(aX) = aE(X)$$
$$\color{green}{E \left( \left(\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right) \left(\boldsymbol{x}_{n} - \boldsymbol{\hat{x}}_{n,n-1} \right)^{T} \right) = \boldsymbol{P}_{n,n-1}}$$
$$\color{blue}{ E \left( \boldsymbol{v}_{n}\boldsymbol{v}_{n}^{T}\right) = \boldsymbol{R}_{n}}$$
$$\color{green}{\boldsymbol{P}_{n,n-1}}$$ is the prior estimate covariance
$$\color{blue}{\boldsymbol{R}_{n}}$$ is the measurement covariance
$$\boldsymbol{P}_{n,n} = \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right) \color{green}{\boldsymbol{P}_{n,n-1} } \left(\boldsymbol{I} - \boldsymbol{K}_{n}\boldsymbol{H} \right)^{T} + \boldsymbol{K}_{n} \color{blue}{ \boldsymbol{R}_{n}} \boldsymbol{K}_{n}^{T}$$ Covariance Update Equation!