Table 1 Kalman filter

Model

${\mathbf{x}}_k=A_{k-1}{\mathbf{x}}_{k-1}+B_{k-1}{\mathbf{u}}_{k-1}+W_{k-1}{\mathbf{w}}_{k-1}$, ${\mathbf{w}}_{k-1}\sim \mathcal{N}(0,Q_{k-1})$, ${\mathbf{y}}_k=H_k{\mathbf{x}}_k+V_k{\mathbf{v}}_k$, ${\mathbf{v}}_k\sim \mathcal{N}(0,R_k)$

Initialization

${\hat{\mathbf{x}}}_0$, $P_0=E[{\hat{\mathbf{x}}}_0{\hat{\mathbf{x}}}_0^T]$

Predictor

${\hat{\mathbf{x}}}_k^{-}=A_{k-1}{\hat{\mathbf{x}}}_{k-1}+B_{k-1}{\mathbf{u}}_{k-1}$, $P_k^{-}=A_{k-1}{P}_{k-1}{A}_{k-1}^T+W_{k-1}{Q}_{k-1}{W}_{k-1}^T$

Kalman gain

$K_k=P_k^{-}{H}_k^T{[H_k{P}_k^{-}{H}_k^T+V_k{R}_k{V}_k^T]}^{-1}$

Corrector

${\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{-}+K_k({\mathbf{y}}_k-H_k{\hat{\mathbf{x}}}_k^{-})$, $P_k=(I-K_k{H}_k)P_k^{-}$