Algorithm 2: Crassidis et al. [6]

Step 1: Initialize $\theta$ and $k=0$ Step 2: for each measurement do:                 Calculate $z_k$ (4)                 Extract $D_k$ and $h_k$ from ${\theta }_k$ (16)                 Calculate $S_k,J_k,F_k$ (20) and $H\left({\theta }_k\right)$ (19)                 Calculate Kalman Gain $K_k$ (18)                 Update estimation: ${\theta }_k\leftarrow {\theta }_{k+1}$                 Update covariance matrix: $P_k\leftarrow P_{k+1}$ (18)                 $k\leftarrow k+1$ Step 3: Extract $D^{*}$ and $h^{*}$ from ${\theta }^{*}$ (16) Step 4: Calculate T and h (22)