Algorithm 2: Extended Kalman filter

1: function $\mathrm{EKF}\left({\widehat{\mathit{x}}}_{\mathrm{P},\mathrm{k}-1},{\mathit{P}}_{\mathrm{P},\mathrm{k}-1},{\mathit{y}}_{\mathrm{P},\mathrm{k}},{\mathit{Q}}_{\mathrm{P},\mathrm{k}},{\mathit{R}}_{\mathrm{P},\mathrm{k}}\right)$

$2\text{: }{\widehat{\mathit{x}}}_{\mathrm{P},\mathrm{k}|\mathrm{k}-1}\leftarrow \mathrm{f}\left({\widehat{\mathit{x}}}_{\mathrm{P},\mathrm{k}-1}\right)$                                                               $⊳$ Prediction

$3\text{: }{\mathit{P}}_{\mathrm{P},\mathrm{k}|\mathrm{k}-1}\leftarrow {\mathit{F}}_{\mathrm{P},\mathrm{k}}{\mathit{P}}_{\mathrm{P},\mathrm{k}-1}{\mathit{F}}_{\mathrm{P},\mathrm{k}}^{\mathrm{T}}+{\mathit{Q}}_{\mathrm{P},\mathrm{k}}$

$4\text{: }{\mathit{K}}_{\mathrm{P},\mathrm{k}}\leftarrow {\mathit{P}}_{\mathrm{P},\mathrm{k}|\mathrm{k}-1}{\mathit{H}}_{\mathrm{P},\mathrm{k}}^{\mathrm{T}}{\left({\mathit{H}}_{\mathrm{P},\mathrm{k}}{\mathit{P}}_{\mathrm{P},\mathrm{k}|\mathrm{k}-1}{\mathit{H}}_{\mathrm{P},\mathrm{k}}^{\mathrm{T}}+{\mathit{R}}_{\mathrm{P},\mathrm{k}}\right)}^{-1}$

$5\text{: }{\tilde{\mathit{y}}}_{\mathrm{P},\mathrm{k}}\leftarrow {\mathit{y}}_{\mathrm{P},\mathrm{k}}-\mathrm{h}\left({\widehat{\mathit{x}}}_{\mathrm{P},\mathrm{k}|\mathrm{k}-1}\right)$

$6\text{: }{\widehat{\mathit{x}}}_{\mathrm{P},\mathrm{k}}\leftarrow {\widehat{\mathit{x}}}_{\mathrm{P},\mathrm{k}|\mathrm{k}-1}+{\mathit{K}}_{\mathrm{P},\mathrm{k}}{\tilde{\mathit{y}}}_{\mathrm{P},\mathrm{k}}$                                                     $⊳$ Update

$7\text{: }{\mathit{P}}_{\mathrm{P},\mathrm{k}}\leftarrow \left(\mathbf{I}-{\mathit{K}}_{\mathrm{P},\mathrm{k}}{\mathit{H}}_{\mathrm{P},\mathrm{k}}\right){\mathit{P}}_{\mathrm{P},\mathrm{k}|\mathrm{k}-1}{\left(\mathbf{I}-{\mathit{K}}_{\mathrm{P},\mathrm{k}}{\mathit{H}}_{\mathrm{P},\mathrm{k}}\right)}^{\mathrm{T}}+{\mathit{K}}_{\mathrm{P},\mathrm{k}}{\mathit{R}}_{\mathrm{P},\mathrm{k}}{\mathit{K}}_{\mathrm{P},\mathrm{k}}^{\mathrm{T}}$

8:      return ${\widehat{\mathit{x}}}_{\mathrm{P},\mathrm{k}}$$,{\mathit{P}}_{\mathrm{P},\mathrm{k}}$$,{\tilde{\mathit{y}}}_{\mathrm{P},\mathrm{k}}$

9: end function