# Cooperative Localization Using Distance Measurements for Mobile Nodes
## 4 Proposed Algorithm Robust to Ambiguity and Noise
The objective function based on the least square estimator is given by  
\[Obj\big{(}\theta;\ \mathbf{\mathcal{S}}^{*},\Delta\mathbf{\mathcal{S}}^{\prime}, \mathbf{D}^{\prime}\big{)}=\sum_{i=1}^{n}(a_{i}+b_{i}\cos(\theta)+c_{i}\sin( \theta))^{2}. \tag{37}\]  
Solutions to (37) can be obtained by taking the derivative of the objective function with respect to \(\theta\) and equating to zero, from this we obtain a quartic equation (see Appendix B for details), which has, at most, four real roots giving the corresponding collection of angles as  
\[\mathbf{\theta}_{\lambda}=\big{\{}\arcsin(\lambda_{1}),g(\pi-\arcsin(\lambda_{1} )),\ldots,\arcsin(\lambda_{m}),g(\pi-\arcsin(\lambda_{m}))\big{\}}\in[-\pi,\pi),\]  
where \(\lambda_{j}\in[-1,1]\), \(j=1,\ldots,m\), and \(m\) is the number of solutions, such that \(1\leq m\leq 4\). Subsequently, (37) becomes  
\[\widehat{\theta}_{R}\Big{(}\mathbf{\mathcal{S}}^{*},\Delta\mathbf{\mathcal{S}}^{ \prime},\widehat{\mathbf{D}}^{\prime}\Big{)}=\arg\min_{\theta\in\mathbf{\theta}_{ \lambda}}Obj\Big{(}\theta;\ \mathbf{\mathcal{S}}^{*},\Delta\mathbf{\mathcal{S}}^{ \prime},\widehat{\mathbf{D}}^{\prime}\Big{)}, \tag{38}\]  
and, accordingly, by (30)  
\[\mathbf{\mathcal{S}}^{**}=\mathbf{M}\Big{(}\widehat{\theta}_{R}\Big{(}\mathbf{\mathcal{ S}}^{*},\Delta\mathbf{\mathcal{S}}^{\prime},\widehat{\mathbf{D}}^{\prime}\Big{)} \Big{)}\mathbf{\mathcal{S}}^{*}. \tag{39}\]In order to detect flip ambiguity, we can follow the same method, as described in Section 3, i.e., let the lead node move to another position \([\Delta x_{1}^{\prime\prime},\Delta y_{1}^{\prime\prime}]^{T}\) and solve \(\widehat{\theta}_{R}(\mathbf{\mathcal{S}}^{**},\Delta\mathbf{\mathcal{S}}^{\prime \prime},\widehat{\mathbf{\mathsf{D}}}^{\prime\prime})\) via (38) while using the estimated distance matrix \(\widehat{\mathbf{\mathsf{D}}}^{\prime\prime}\) obtained at the new position \(\mathbf{\mathcal{S}}^{\prime\prime}=\mathbf{\mathcal{S}}+\Delta\mathbf{\mathcal{S}}^{ \prime\prime}\). However, becasue of the presence of noise \(\widehat{\theta}_{R}\big{(}\mathbf{\mathcal{S}}^{**},\Delta\mathbf{\mathcal{S}}^{ \prime\prime},\widehat{\mathbf{\mathsf{D}}}^{\prime\prime}\big{)}\) is not necessarily equal to \(0\) when there is no flip ambiguity. To handle this, we need to create a detector for the flip. For this, we use a straightforward threshold detector:  
\[ \begin{cases}\text{no flip},&\text{if }\left|\widehat{\theta}_{R}\big{(}\mathbf{ \mathcal{S}}^{**},\Delta\mathbf{\mathcal{S}}^{\prime\prime},\widehat{\mathbf{\mathsf{ D}}}^{\prime\prime}\big{)}\right|\leq|l|\\ \text{flip},&\text{if }\left|\widehat{\theta}_{R}\big{(}\mathbf{\mathcal{S}}^{**}, \Delta\mathbf{\mathcal{S}}^{\prime\prime},\widehat{\mathbf{\mathsf{D}}}^{\prime\prime }\big{)}\right|>|l|,\end{cases}  \tag{40}\]  
The following proposition, the proof of which is given in Appendix C, allows for us to efficiently find an optimal threshold in (40). **Proposition 1**. : _Under the noise-free case, if there exists flip ambiguity, then_  
\[\theta_{R}\big{(}\mathbf{\mathcal{S}}^{**},\Delta\mathbf{\mathcal{S}}^{\prime\prime}, \mathbf{\mathsf{D}}^{\prime\prime}\big{)}=g\Big{(}-2\operatorname{atan2}(\Delta x _{1}^{\prime},\Delta y_{1}^{\prime})+2\operatorname{atan2}(\Delta x_{1}^{ \prime\prime},\Delta y_{1}^{\prime\prime})\Big{)}, \tag{41}\]  
_where \(g(t)=t-2\pi\Big{\lfloor}\frac{t}{2\pi}+\frac{1}{2}\Big{\rfloor}\)._  
From Proposition 1, under the noise-free case, we know the value of \(\theta_{R}\big{(}\mathbf{\mathcal{S}}^{**},\Delta\mathbf{\mathcal{S}}^{\prime\prime}, \mathbf{\mathsf{D}}^{\prime\prime}\big{)}\) when a flip occurs; therefore, in the noisy case, the optimal threshold is  
\[l=\frac{1}{2}g\Big{(}-2\operatorname{atan2}(\Delta x_{1}^{\prime},\Delta y_{1 }^{\prime})+2\operatorname{atan2}(\Delta x_{1}^{\prime\prime},\Delta y_{1}^{ \prime\prime})\Big{)}.\]  
As a conclusion, the positions of nodes can estimated using the following formula:  
\[\widehat{\mathbf{\mathcal{S}}}=