Algorithm 1 Point-to-sphere ICP algorithm to estimate the position of the UWB tag

1:procedurepointToSphereICP($\mathcal{L},\mathcal{R},t,{\delta }_{min},max\_iter$) 2:    $\mathcal{L}=\{l_1,\dots ,l_N\}$: anchors’ 3D positions 3:    $\mathcal{R}=\{r_1,\dots ,r_N\}$: distances from the tag to the anchors 4:    t: starting estimate of the tag position, such as the last estimate or $(0,0,0)$ the very first time 5:    ${\delta }_{min}$: smallest position update to iterate once more 6:    max_iter: maximum number of iterations to stop ICP 7:    $\delta \leftarrow \infty$, num_iter← 0 8:    while $(\delta >{\delta }_{min}$) and (num_iter< max_iter) do 9:        $\mathcal{C}\leftarrow$getClosestPoints$(\mathcal{L},\mathcal{R},t)$                ▹ closest points obtained from Equation (1) and (2) 10:        $\mathcal{U}\leftarrow \mathcal{L}-\mathcal{C}$                                                ▹ set of 3D translations required for each sphere 11:        mean_update ←average$\left(\mathcal{U}\right)$                        ▹ average update for each axis (from closed-form 12:                                                                                ▹ solution of the underlying least-squares problem) 13:        $t\leftarrow t+$mean_update                                                                ▹ update the 3D position of the tag 14:        $\delta \leftarrow$norm(mean_update)                                                                ▹ L2 norm of the update vector 15:        num_iter ←num_iter+ 1 16:    end while 17:    return t                                                                ▹ return the updated 3D position of the tag 18:end procedure