KF Algorithm

NOTE: Sometimes is possible to find $z(t)$ instead of $y(t)$ to indicate the measurement at time $t$.

---

0. Initialization: arbitrarily choose $\hat{x}(0|-1)$, and $P(0|-1)$
1. CORRECTION STEP:

$$\begin{array}{rl}& S=CP(t|t-1)C^T+R\\ & K=P(t|t-1)C^T{S}^{-1}\\ & \hat{x}(t|t)=\hat{x}(t|t-1)+K\left(y(t)-h(\hat{x}(t|t-1))\right)\\ & P(t|t)=\left[I-KC\right]P(t|t-1)\end{array}$$

1. PREDICTION STEP:

$$\begin{array}{rl}& \hat{x}(t+1|t)=A\hat{x}(t|t)+Bu(t)\\ & P(t+1|t)=AP(t|t)A^T+GQ{G}^T\end{array}$$

3. Iterate.

NOTE: Depending on the initial data the prediction step and the correction step can be inverted.