Kalman filter a posteriori estimate covariance matrix

[ryan88]

I am having some trouble deriving the a posteriori estimate covariance matrix for the linear Kalman filter. Below I have shown my workings for two methods. Method one is fine and gives the expected result. Method two is the way I tried to derive it initially before further expanding out terms to arrive at method one.

For completeness I have also included the equations for the model and the prediction and update stages of the filter.

Model
$\mathbf{x}_{k}=\mathbf{A}_{k}\mathbf{x}_{k-1}+\mathbf{B}_{k}\mathbf{u}_{k}+\mathbf{G}_{k}\mathbf{q}_{k}$
$\mathbf{z}_{k}=\mathbf{H}_{k}\mathbf{x}_{k}+\mathbf{r}_{k}$

Prediction
$\hat{\mathbf{x}}_{k|k-1}=\mathbf{A}_{k}\hat{\mathbf{x}}_{k-1|k-1}+\mathbf{B}_{k}\mathbf{u}_{k}$
$\hat{\mathbf{z}}_{k|k-1}=\mathbf{H}_{k}\hat{\mathbf{x}}_{k|k-1}$
$\mathbf{P}_{k|k-1}=\text{cov}\left(\mathbf{x}_{k}-\hat{\mathbf{x}}_{k|k-1}\right)=\mathbf{A}_{k}\mathbf{P}_{k-1|k-1}\mathbf{A}_{k}^{\text{T}}+\mathbf{G}_{k}\mathbf{Q}_{k}\mathbf{G}_{k}^{\text{T}}$

Update
$\hat{\mathbf{x}}_{k|k}=\hat{\mathbf{x}}_{k|k-1}+\mathbf{K}_{k}\left(\mathbf{z}_{k}-\hat{\mathbf{z}}_{k|k-1}\right)$
$\mathbf{S}_{k}=\text{cov}\left(\mathbf{z}_{k}-\hat{\mathbf{z}}_{k|k-1}\right)=\mathbf{H}_{k}\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{\text{T}}+\mathbf{R}_{k}$

A posteriori estimate covariance - method one
Definition of the a posteriori estimate covariance matrix:
$\mathbf{P}_{k|k}=\text{cov}\left(\mathbf{x}_{k}-\hat{\mathbf{x}}_{k|k}\right)$
Substituting for $\hat{\mathbf{x}}_{k|k}$:
$\mathbf{P}_{k|k}=\text{cov}\left(\mathbf{x}_{k}-\hat{\mathbf{x}}_{k|k-1}-\mathbf{K}_{k}\left(\mathbf{z}_{k}-\hat{\mathbf{z}}_{k|k-1}\right)\right)$
Substituting for $\mathbf{z}_{k}$ and $\hat{\mathbf{z}}_{k|k-1}$:
$\mathbf{P}_{k|k}=\text{cov}\left(\mathbf{x}_{k}-\hat{\mathbf{x}}_{k|k-1}-\mathbf{K}_{k}\left(\mathbf{H}_{k}\mathbf{x}_{k}+\mathbf{r}_{k}-\mathbf{H}_{k}\hat{\mathbf{x}}_{k|k-1}\right)\right)$
Collecting like terms:
$\mathbf{P}_{k|k}=\text{cov}\left(\left(\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k}\right)\left(\mathbf{x}_{k}-\hat{\mathbf{x}}_{k|k-1}\right)-\mathbf{K}_{k}\mathbf{r}_{k}\right)$
Measurement noise and the a priori state estimate are uncorrelated:
$\mathbf{P}_{k|k}=\left(\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k}\right)\text{cov}\left(\mathbf{x}_{k}-\hat{\mathbf{x}}_{k|k-1}\right)\left(\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k}\right)^{\text{T}}+\mathbf{K}_{k}\text{cov}\left(\mathbf{r}_{k}\right)\mathbf{K}_{k}^{\text{T}}$
Substitute for $\mathbf{P}_{k|k-1}$ and use definition of $\mathbf{R}_{k}$:
$\mathbf{P}_{k|k}=\left(\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k}\right)\mathbf{P}_{k|k-1}\left(\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k}\right)^{\text{T}}+\mathbf{K}_{k}\mathbf{R}_{k}\mathbf{K}_{k}^{\text{T}}$
Expanding out terms:
$\mathbf{P}_{k|k}=\mathbf{P}_{k|k-1}-\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{\text{T}}\mathbf{K}_{k}^{\text{T}}-\mathbf{K}_{k}\mathbf{H}_{k}\mathbf{P}_{k|k-1}+\mathbf{K}_{k}\left(\mathbf{H}_{k}\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{\text{T}}+\mathbf{R}_{k}\right)\mathbf{K}_{k}^{\text{T}}$
Substituting for $\mathbf{S}_{k}$:
$\mathbf{P}_{k|k}=\mathbf{P}_{k|k-1}-\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{\text{T}}\mathbf{K}_{k}^{\text{T}}-\mathbf{K}_{k}\mathbf{H}_{k}\mathbf{P}_{k|k-1}+\mathbf{K}_{k}\mathbf{S}_{k}\mathbf{K}_{k}^{\text{T}}$
Minimize by taking derivative and setting to zero:
$\frac{\partial\;\text{tr}\left(\mathbf{P}_{k|k}\right)}{\partial\;\mathbf{K}_{k}}=-2\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{\text{T}}+2\mathbf{K}_{k}\mathbf{S}_{k}=0$
Rearrange for $\mathbf{K}_{k}$:
$\mathbf{K}_{k}=\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{\text{T}}\mathbf{S}_{k}^{-1}$
Multiply both sides by $\mathbf{S}_{k}\mathbf{K}_{k}^{\text{T}}$:
$\mathbf{K}_{k}\mathbf{S}_{k}\mathbf{K}_{k}^{\text{T}}=\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{\text{T}}\mathbf{S}_{k}^{-1}\mathbf{S}_{k}\mathbf{K}_{k}^{\text{T}}$
Substitute back into $\mathbf{P}_{k|k}$:
$\mathbf{P}_{k|k}=\mathbf{P}_{k|k-1}-\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{\text{T}}\mathbf{K}_{k}^{\text{T}}-\mathbf{K}_{k}\mathbf{H}_{k}\mathbf{P}_{k|k-1}+\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{\text{T}}\mathbf{K}_{k}^{\text{T}}$
Collect terms:
$\mathbf{P}_{k|k}=\left(\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k}\right)\mathbf{P}_{k|k-1}$

A posteriori estimate covariance - method two
Definition of the a posteriori estimate covariance matrix:
$\mathbf{P}_{k|k}=\text{cov}\left(\mathbf{x}_{k}-\hat{\mathbf{x}}_{k|k}\right)$
Substituting for $\hat{\mathbf{x}}_{k|k}$:
$\mathbf{P}_{k|k}=\text{cov}\left(\mathbf{x}_{k}-\hat{\mathbf{x}}_{k|k-1}-\mathbf{K}_{k}\left(\mathbf{z}_{k}-\hat{\mathbf{z}}_{k|k-1}\right)\right)$
Measurement error and state error are uncorrelated:
$\mathbf{P}_{k|k}=\text{cov}\left(\mathbf{x}_{k}-\hat{\mathbf{x}}_{k|k-1}\right)+\mathbf{K}_{k}\text{cov}\left(\mathbf{z}_{k}-\hat{\mathbf{z}}_{k|k-1}\right)\mathbf{K}_{k}^{\text{T}}$
Substitute for $\mathbf{P}_{k|k-1}$ and $\mathbf{S}_{k}$
$\mathbf{P}_{k|k}=\mathbf{P}_{k|k-1}+\mathbf{K}_{k}\mathbf{S}_{k}\mathbf{K}_{k}^{\text{T}}$
Minimize by taking derivative and setting to zero:
$\frac{\partial\;\text{tr}\left(\mathbf{P}_{k|k}\right)}{\partial\;\mathbf{K}_{k}}=2\mathbf{K}_{k}\mathbf{S}_{k}=0$

These two approaches should be the same, but are clearly not giving the same results. Any ideas what I am doing wrong? I just can't see it...

 

[jvicens]

First of all, it's important to note that both methods are valid and will give the same result. However, there is a small mistake in the second method that is causing the discrepancy.

In the second method, when substituting for \mathbf{P}_{k|k-1}, the term \mathbf{K}_{k}\mathbf{S}_{k}\mathbf{K}_{k}^{\text{T}} should be replaced with \mathbf{K}_{k}\mathbf{H}_{k}\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{\text{T}}\mathbf{K}_{k}^{\text{T}}. This is because the measurement error and state error are uncorrelated, but the measurement and state variables are correlated through the Kalman gain.

Making this correction should lead to the same result as the first method.