Dismiss Notice
Join Physics Forums Today!
The friendliest, high quality science and math community on the planet! Everyone who loves science is here!

Kalman filter a posteriori estimate covariance matrix

  1. Aug 14, 2015 #1
    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
    [itex]\mathbf{x}_{k}=\mathbf{A}_{k}\mathbf{x}_{k-1}+\mathbf{B}_{k}\mathbf{u}_{k}+\mathbf{G}_{k}\mathbf{q}_{k}[/itex]
    [itex]\mathbf{z}_{k}=\mathbf{H}_{k}\mathbf{x}_{k}+\mathbf{r}_{k}[/itex]

    Prediction
    [itex]\hat{\mathbf{x}}_{k|k-1}=\mathbf{A}_{k}\hat{\mathbf{x}}_{k-1|k-1}+\mathbf{B}_{k}\mathbf{u}_{k}[/itex]
    [itex]\hat{\mathbf{z}}_{k|k-1}=\mathbf{H}_{k}\hat{\mathbf{x}}_{k|k-1}[/itex]
    [itex]\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}}[/itex]

    Update
    [itex]\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)[/itex]
    [itex]\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}[/itex]

    A posteriori estimate covariance - method one
    Definition of the a posteriori estimate covariance matrix:
    [itex]\mathbf{P}_{k|k}=\text{cov}\left(\mathbf{x}_{k}-\hat{\mathbf{x}}_{k|k}\right)[/itex]
    Substituting for [itex]\hat{\mathbf{x}}_{k|k}[/itex]:
    [itex]\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)[/itex]
    Substituting for [itex]\mathbf{z}_{k}[/itex] and [itex]\hat{\mathbf{z}}_{k|k-1}[/itex]:
    [itex]\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)[/itex]
    Collecting like terms:
    [itex]\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)[/itex]
    Measurement noise and the a priori state estimate are uncorrelated:
    [itex]\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}}[/itex]
    Substitute for [itex]\mathbf{P}_{k|k-1}[/itex] and use definition of [itex]\mathbf{R}_{k}[/itex]:
    [itex]\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}}[/itex]
    Expanding out terms:
    [itex]\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}}[/itex]
    Substituting for [itex]\mathbf{S}_{k}[/itex]:
    [itex]\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}}[/itex]
    Minimize by taking derivative and setting to zero:
    [itex]\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[/itex]
    Rearrange for [itex]\mathbf{K}_{k}[/itex]:
    [itex]\mathbf{K}_{k}=\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{\text{T}}\mathbf{S}_{k}^{-1}[/itex]
    Multiply both sides by [itex]\mathbf{S}_{k}\mathbf{K}_{k}^{\text{T}}[/itex]:
    [itex]\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}}[/itex]
    Substitute back in to [itex]\mathbf{P}_{k|k}[/itex]:
    [itex]\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}}[/itex]
    Collect terms:
    [itex]\mathbf{P}_{k|k}=\left(\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k}\right)\mathbf{P}_{k|k-1}[/itex]

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

    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. jcsd
  3. Aug 19, 2015 #2
    Thanks for the post! This is an automated courtesy bump. Sorry you aren't generating responses at the moment. Do you have any further information, come to any new conclusions or is it possible to reword the post?
     
Know someone interested in this topic? Share this thread via Reddit, Google+, Twitter, or Facebook




Similar Discussions: Kalman filter a posteriori estimate covariance matrix
  1. Kalman filters (Replies: 3)

  2. Covariance Matrix (Replies: 7)

Loading...