Table 3 Unscented Kalman filter

Model

${\mathbf{x}}_k=\mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{u}}_{k-1},{\mathbf{w}}_{k-1})=\tilde{\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)=\tilde{\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]$

Weights

$W_i^{\mathrm{mean}}$, $W_i^{\mathrm{cov}}$, i = 0,…,p

Sigma points

${\mathcal{X}}_{k-1}^{(i)}={\mathcal{X}}_{k-1}^{(i)}({\hat{\mathbf{x}}}_{k-1},P_{k-1})$, i = 0,…,p

Predictor

${{\mathcal{X}}_k^{-}}^{(i)}=\mathbf{f}({\mathcal{X}}_{k-1}^{(i)},{\mathbf{u}}_{k-1},0)$, i = 0,…,p, ${\hat{\mathbf{x}}}_k^{-}={\sum }_{i=0}^p{W}_i^{\mathrm{mean}}{{\mathcal{X}}_k^{-}}^{(i)}$, $P_k^{-}={\sum }_{i=0}^p{W}_i^{\mathrm{cov}}({{\mathcal{X}}_k^{-}}^{(i)}-{\hat{\mathbf{x}}}_k^{-}){({{\mathcal{X}}_k^{-}}^{(i)}-{\hat{\mathbf{x}}}_k^{-})}^T+Q_{k-1}$

Predicted observation

${\mathcal{Y}}_k^{(i)}=\mathbf{h}({{\mathcal{X}}_k^{-}}^{(i)},0)$, ${\hat{\mathbf{y}}}_k^{-}={\sum }_{i=0}^p{W}_i^{\mathrm{mean}}{\mathcal{Y}}_k^{(i)}$, i = 0,…,p, $P_k^{yy}={\sum }_{i=0}^p{W}_i^{\mathrm{cov}}({\mathcal{Y}}_k^{(i)}-{\hat{\mathbf{y}}}_k^{-}){({\mathcal{Y}}_k^{(i)}-{\hat{\mathbf{y}}}_k^{-})}^T+R_k$, $P_k^{xy}={\sum }_{i=0}^p{W}_i^{\mathrm{cov}}({{\mathcal{X}}_k^{-}}^{(i)}-{\hat{\mathbf{x}}}_k^{-}){({\mathcal{Y}}_k^{(i)}-{\hat{\mathbf{y}}}_k^{-})}^T$

Kalman gain

$K_k=P_k^{xy}{(P_k^{yy})}^{-1}$

Corrector

${\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{-}+K_k({\mathbf{y}}_k-{\hat{\mathbf{y}}}_k^{-})$, $P_k=P_k^{-}-K_k{P}_k^{yy}{K}_k^T$