Kalman filtering

Matthieu Bloch

Tuesday, October 11, 2022

Today in ECE 6555

  • Don't forget
    • Problem Set 3 due Thursday October 13, 2022 on Gradescope
      • Typos corrected
    • Office hours today
  • Last time
    • First lemmas for Kalman filtering
  • Today's plan
    • More Kalman filtering
  • Questions?

Standard state space model

  • The standard state space model is of the form \[ \vecx_{i+1} = \matF_i\vecx_i + \matG_i\vecu_i\qquad \vecy_i = \matH_i \vecx_i + \vecv_i \] with known matrices \(\set{\matF_i,\matG_i,\matH_i}\) and \[ \dotp{\left[\begin{array}{c}\vecx_0\\\vecu_i\\\vecv_i\end{array}\right]}{\left[\begin{array}{c}\vecx_0\\\vecu_j\\\vecv_j\\1\end{array}\right]} \eqdef \left[\begin{array}{cccc}\Pi_0&0&0&0\\0&\matQ_i\delta_{ij}&\matS_i\delta_{ij}&0\\0&\matS_i^T\delta_{ij}&\matR_i\delta_{ij}&0\end{array}\right] \]
    • \(u_i\) is called the process noise, \(v_i\) is called the measurement noise
    • By convention, \(\vecx_i\in\bbR^n\), \(\vecy_i\in\bbR^m\), \(\matF_i\in\bbR^{n\times n}\), \(\matH_i\in\bbR^{m\times n}\), \(\matG_i\in\bbR^{n\times p}\)
  • State space models ofen appear "naturally" from physics (more on this later) and differential equations
  • They provide a very useful description that can be exploited for estimation

Standard state-space model: Properties

    • For \(i\geq j\) \(\dotp{\vecu_i}{\vecx_j}=0\) and \(\dotp{\vecv_i}{\vecx_j}=0\)
    • For \(i> j\) \(\dotp{\vecu_i}{\vecy_j}=0\) and \(\dotp{\vecv_i}{\vecy_j}=0\)
    • For \(i=j\) \(\dotp{\vecu_i}{\vecy_j}=\matS_i\) and \(\dotp{\vecv_i}{\vecy_j}=\matR_i\)
    • If \(\matF_i\) non singular, \(\dotp{\vecu_i}{\vecx_0}=0\) if and only if \(\forall i\geq 0\) \(\dotp{\vecu_i}{\vecx_i}=0\)

  • Let \(\dotp{\vecx_i}{\vecx_i}=\Pi_i\). Then \[ \forall i\geq 0\qquad \Pi_{i+1} = \matF_i\Pi_i\matF_i^T + \matG_i\matQ_i\matG_i^T \] Let \(\Phi(i,j)\eqdef \prod_{\ell=i-1}^j\matF_\ell\) for \(i>j\) and \(\Phi(i,i)=\matI\). Then \[ \dotp{\vecx_i}{\vecx_j} = \begin{cases}\Phi(i,j)\Pi_j\text{ for }i\geq j\\\Pi_i\Phi(j,i)^T\text{ for }i\leq j\end{cases} \] \[ \dotp{\vecy_i}{\vecy_j} = \begin{cases}\matH_i\Phi(i,j+1)\matN_j\text{ for }i> j\text{ with }\matN_i=\matF_i\Pi_i\matH_i^T+\matG_i\matS_i\\ \matR_i+\matH_i\Pi_i\matH_i^T\text{ for }i= j\\ \matN_i^T\Phi(j,i+1)^T\matH)j^T\text{ for }i< j\end{cases} \]

Recursion for innovation process

  • Objective: come up with a computationally efficient way of performing estimation

  • The innovation \(\vece_i\) is \[ \vece_i = \vecy_i - \matH_i\hat{\vecx}_{i|i-1} \]

  • The one step predition \(\hat{\vecx}_{i|i-1}\) can be computed as \[ \hat{\vecx}_{i+1|i} = \matF_i\hat{\vecx}_{i|i-1}+\matK_{p,i}\vece_i\quad\textsf{where } \matK_{p,i} = \dotp{\vecx_{i+1}}{\vece_i}\norm{\vece_i}^{-2} \]

  • Set \(\hat{\vecx}_i\eqdef\hat{\vecx}_{i|i-1}\), \(\tilde{\vecx}_i=\vecx_i-\hat{\vecx}_{i}\) and \(\matP_i \eqdef \dotp{\tilde{\vecx}_{i}}{\tilde{\vecx}_{i}}\) \[ \norm{\vece_i}^{2} = \matH_i\matP_i\matH_i^T+\matR_i \] \[ \matK_{p,i} = (\matF_i\matP_i\matH_i^T+\matG_i\matS_i)\norm{\vece_i}^{-2} \]

Recursion for innovation process

  • Recursion for \(\matP_i\): \[ \matP_{i+1} = \matF_i\matP_i\matF_i^T + \matG_i\matQ_i\matG_i^T - \matK_{p,i}\norm{\vece_i}^{2}\matK_{p,i}^T \]

  • Since \(\tilde{\vecx}_0=\vecx_0\), we have \(\matP_0=\Pi_0\)
  • Innovation model for output process
    • We can rearrange the equations to obtain
    \[ \hat{\vecx}_{i+1} = \matF_i\vecx_i+\matK_{p,i}\vece_i\qquad \vecy_i=\matH_i\hat{\vecx}_i+\vece_i \]
    • This is called the innovation representation of the process \(\set{\vecy_i}\)

Predicted and Filtered Estimators

  • \[\hat{\vecx}_{i+1} = \matF_{p,i}\hat{\vecx}_{i}+\matK_{p,i}\vecy_i\quad\textsf{where } \matF_{p,i} = \matF_i-\matK_{p,i}\matH_i\] with the recursions for \(\matK_{p,i}\), \(\norm{\vece_i}\), \(\matP_i\).

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} = ? \]