Table 2 Extended Kalman filter

Model

${\mathbf{x}}_k=\mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{u}}_{k-1},{\mathbf{w}}_{k-1})$, ${\mathbf{w}}_{k-1}\sim \mathcal{N}(0,Q_{k-1})$, ${\mathbf{y}}_k=\mathbf{h}({\mathbf{x}}_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^{-}=\mathbf{f}({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_{k-1},0)$, $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-\mathbf{h}({\hat{\mathbf{x}}}_k^{-},0))$, $P_k=(I-K_k{H}_k)P_k^{-}$