SI&DA - Lecture 23 'Derivation of the Kalman Filter - Part II'

Summary

The KF (Kalman Filter) Algorithm Initialization: select $\hat{x}(0|-1)$ and $P(0|-1)$. For-Loop: $t=0,1,2,\dots$ \kern25px$$K(t) = P(t \ | \ t-1) \cdot C^T \cdot \left[C \kern1px P(t \ | \ t-1) \kern1px C^T + R \right]^{-1} \kern25px$$\hat{x}(t \ | \ t-1) = \hat{x}(t \ | \ t-1) + K(t)\cdot \left(y(t) - C\kern1px \hat{x}(t \ | \ t-1)\right) \kern25px$$P(t \ | \ t) = \left[I - K(t) \kern1px C\right]\cdot P(t \ | \ t-1) \kern25px$$\hat{x}(t+1 \ | \ t) = A \kern1px \hat{x}(t \ | \ t) + B \kern1px u(t) \kern25px$$P(t+1 \ | \ t) = A \kern2px P(t \ | \ t) \kern1px A^T + G \kern1px Q \kern1px G^T

The Wikipedia version of the algorithm: KF Algorithm (Wikipedia)

NOTE: We propagate also $P(t|t)$ from the past iteration of the loop not only $\hat{x}(t|t)$. Think of $P$ as the amount of uncertainty we have for the prediction $\hat{x}$.

---

NOTE: During the Lab session the professor doesn’t use the eigenvalues of $P$ instead, it just takes the elements of the diagonal of $P$: $p_{1,1}$, $p_{2,2}$, $p_{3,3}$, $\dots$