Kalman filtering

Matthieu Bloch

Thursday, October 13, 2022

Today in ECE 6555

  • Don't forget
    • Problem Set 3 due Friday October 14, 2022 on Gradescope
      • Typos corrected
      • Lots of hints provided in office hours but work the problems nevertheless
  • Last time
    • Kalman filter equations!
  • Today's plan
    • Time-update
  • Questions?

Measurement and time udpdate form

  • Objective: decompose problem into measurement update to go from :
    • \(\hat{\vecx}_{i|i-1}\) to \(\hat{\vecx}_{i|i}\)
    • \(\hat{\vecx}_{i|i}\) to \(\hat{\vecx}_{i+1|i}\)

  • Assume we have computed \(\hat{\vecx}_{i}\eqdef\hat{\vecx}_{i|i-1}\) and we get a new measurement \(\vecy_i\). Then \[ \hat{\vecx}_{i|i} = \hat{\vecx}_{i} + \matK_{f,i}\vece_i\quad\textsf{ with }\matK_{f,i}=\matP_i\matH_i^T\norm{\vece_i}^{-2} \] \[ \matP_{i|i}\eqdef\norm{\vecx_i-\hat{\vecx}_{i|i}}^2 = \matP_i-\matP_i\matH_i^T\norm{\vece_i}^{-2}\matH_i\matP_i \]

  • Assume we have computed \(\hat{\vecx}_{i|i}\) and \(\matP_{i|i}\). Then, \[ \hat{\vecx}_{i+1} = \matF_i\hat{\vecx}_{i|i} + \matG_{i}\hat{\vecu}_{i|i}\quad\textsf{ with }\hat{\vecu}_{i|i}=\matS_i\norm{\vece_i}^{-2}\vece_i \] \[ \matP_{i+1} = \matF_i\matP_{i|i}\matF_i^T+\matG_i(\matQ_i-\matS_i\norm{\vece_i}^2\matS_i^T)\matG_i^T-\matF_i\matK_{f,i}\matS_i^T\matG_i^T-\matG_i\matS_i\matK_{f,i}^T\matF_i^T \]

When is the innovation covariance matrix non singular?

  • If \(\matR_i\succ 0\) then \(\norm{\vece_i}^2\succ 0\)

  • The assumption \(\matR_i\succ 0\) gives us even more

  • Recall time update equations for \(\matS_i=0\) \[ \hat{\vecx}_{i+1} = \matF_i\hat{\vecx}_{i|i} \] \[ \matP_{i+1} = \matF_i\matP_{i|i}\matF_i^T+\matG_i\matQ_i\matG_i^T \]

  • If \(\matR_i\succ 0\), define \(\vecu_{i,s}\eqdef\vecu_i-\matS_i\matR_i^{-1}\vecv_i\). Then, \[ \matP_{i+1} = \matF_{i,s}\matP_{i|i}\matF_{i,s}^T+\matG_i\matQ_{i,s}\matG_i^T \] where \(\matF_{i,s}\eqdef \matF_i-\matG_i\matS_i\matR_i^{-1}\matH_i\) and \(\matQ_{i,s}=\matQ_i-\matS_i\matR_i^{-1}\matS_i^T\)

Information form

  • If \(\matR_i\succ 0\) and \(\matP_i\succ 0\) then \(\matP_{i|i}^{-1}=\matP_{i}^{-1}+\matH_i^T\matR_i^{-1}\matH_i\) and \[ \matP_{i|i}^{-1}\hat{\vecx}_{i|i} = \matP_{i-1}\hat{\vecx}_i+\matH_i^T\matR_i^{-1}\vecy_i \]