Algorithm 1: Recursive EKF algorithm

Input : uk,zk,xk−1,Pk−1

Output: x̂k,Pk

Data: f and h from model (6), Qk,Rk

Initialization: x0,P0

for current time k do

Prediction Step:

x̂ = f (x̂k−1, ûk−1, 0)

${\mathbf{A}}_k=\frac{\partial f}{\partial x}|{\widehat{\mathbf{\text{X}}}}_{k-1},{\mathbf{u}}_{k-1},0$

${\mathbf{H}}_k=\frac{\partial h}{\partial x}|{\widehat{\mathbf{\text{X}}}}_k,0$

${\mathbf{W}}_k=\frac{\partial f}{\partial w}|{\widehat{\mathbf{\text{X}}}}_{k-1},{\mathbf{u}}_{k-1},0$

${\mathbf{V}}_k=\frac{\partial h}{\partial \upsilon }|{\widehat{\mathbf{\text{X}}}}_k,0$

$P_k=A_k{P}_{k-1}{A}_k^T+W_k{Q}_{k-1}{W}_k^T$

Correction Step:

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

x̂ = x̂k + Kk [zk − h(x̂k, 0)]

Pk = (I − Kk Hk) Pk

end