# Kalman filter a posteriori estimate covariance matrix

1. Aug 14, 2015

### 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 in to $\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...

2. Aug 19, 2015