Algorithm 1. Iterative positioning algorithm in Step 2

Input: TOA measurements ρi, $i\in \mathcal{N}$; anchor positions ${\mathbf{p}}^{\left(j\right)}$, $j\in {ℳ}_i, {ℳ}_i⸦ℳ$; initial unknowns ${\mathbf{x}}_0 = {[x_{\mathrm{c}|0}, y_{\mathrm{c}|0}, y_{\mathrm{c}|0}, \delta t_0, {\psi }_0]}^T$; maximum iterative number iter; convergence threshold thr > 0; known height h, rolling angle φ, pitch angle θ and the vectors from the antenna i to reference point c expressed in frame b ${\mathbf{l}}_{\mathrm{c}i}^b, i\in \mathcal{N}$; damping coefficient μ > 0; adjustment coefficient λ > 1.

Iteration:

for q = 1: iter

Calculate the position of each antenna:

${\hat{\mathbf{p}}}_i=f_i(x_{\mathrm{c}|q-1}, y_{\mathrm{c}|q-1}, {\psi }_{q-1}), i\in \mathcal{N}.$

Calculate residual: $\delta {\rho }_{ij}={\rho }_{ij}-\mathbf{\Vert }{\mathbf{p}}^{\left(j\right)}-{\hat{\mathbf{p}}}_i\mathbf{\Vert }-\delta t_{q-1}, i\in \mathcal{N}, ,j\in {ℳ}_i, {ℳ}_i⸦ℳ.$

Calculate uij and wij according to (23) and (41), respectively.

Form H and δz according to (42).

Calculate Q′ according to (46).

Calculate unknown incremental: $\delta {\mathbf{x}}_q ={{\mathbf{Q}}^{\mathbf{\prime }}}^{-1}{H}^T\delta \mathbf{z}.$

Update unknown estimate: ${\mathbf{x}}_q={\mathbf{x}}_{q-1}+\delta {\mathbf{x}}_q.$

if ‖δxq‖ < thr

exit;

else if ‖δxq‖ < ‖δxq−1‖

$\mu =\frac{\mu }{\lambda };$

else

μ = μλ.

end if

end if

end for

Output: x q