# Cooperative Localization Using Distance Measurements for Mobile Nodes
## 4 Proposed Algorithm Robust to Ambiguity and Noise  
The analysis in the previous section assumes ideal measurements; however, in practice, the measurements are corrupted by noise that can have a significant impact on the localization performance. If we consider the distances between two nodes, in the noise-free case the distance between the \(i\)th and \(j\)th nodes is the same regardless of which node it is measured from, i.e., \(d_{i,j}=d_{j,i}\). When noise is introduced, this is no longer the case, if we denote the measured distances as \(\tilde{d}_{i,j}\) and \(\tilde{d}_{j,i}\), then \(\tilde{d}_{i,j}\
eq\tilde{d}_{j,i}\) resulting in uncertainty in our estimates of the distances. In general, the measured distance between the \(i\)th and \(j\)th nodes can be modeled by  
\[\tilde{d}_{i,j}=d_{i,j}+\omega_{i,j},\qquad i,j=1,\ldots,n,\quad i\
eq j, \tag{33}\]  
where \(d_{i,j}\) is the true distance and \(\omega_{i,j}\) is the measurement noise. Accordingly, the noisy Euclidean distance matrix (EDM) can be written into  
\[\bar{\mathbf{D}}= \begin{bmatrix}0&\bar{d}_{1,2}^{2}&\bar{d}_{1,3}^{2}&\cdots&\bar {d}_{1,n}^{2}\\ \bar{d}_{2,1}^{2}&0&\bar{d}_{2,3}^{2}&\cdots&\bar{d}_{2,n}^{2}\\ \bar{d}_{3,1}^{2}&\bar{d}_{3,2}^{2}&0&\cdots&\bar{d}_{3,n}^{2}\\ \vdots&\vdots&\vdots&\ddots&\vdots\\ \bar{d}_{n,1}^{2}&\bar{d}_{n,2}^{2}&\bar{d}_{n,3}^{2}&\cdots&0\end{bmatrix} . \tag{34}\]  
As we know that \(d_{i,j}=d_{j,i}\) when the noise is absent, therefore, in order to obtain a symmetric EDM in noisy case, we can use  
\[\hat{d}_{i,j}=\hat{d}_{j,i}=\mathbb{E}(\bar{d}_{i,j},\bar{d}_{j,i}) \tag{35}\]  
where \(\mathbb{E}(\bar{d}_{i,j},\bar{d}_{j,i})\) is an estimator for the distance between \(i\)-th and \(j\)-th nodes using \(\bar{d}_{i,j}\) and \(\bar{d}_{j,i}\). In practice, this estimator is designed by using the knowledge of noise distribution. For example, when the noise in \(\bar{d}_{i,j}\) and \(\bar{d}_{j,i}\) are assumed to comprise independent and identically distributed (i.i.d.) normal distributions, then we have \(\hat{d}_{i,j}=\hat{d}_{j,i}=\frac{1}{2}\big{(}\bar{d}_{i,j}+\bar{d}_{j,i}\big{)}\)[40]. Because the study of the estimator \(\mathbb{E}(\bar{d}_{i,j},\bar{d}_{j,i})\) and estimation of the EDM is outside the scope of this article, we refer the interested reader to [40; 41] for more information, including completing and estimating an EDM. Accordingly, the estimated symmetric EDM \(\widehat{\mathbf{D}}\) can be obtained while using \(\hat{d}_{i,j}\): Similarly, we can obtain \(\widehat{\mathbf{D}}^{\prime}\) by using the new position of \(\mathbf{\mathcal{S}}^{\prime}=\mathbf{\mathcal{S}}+\Delta\mathbf{\mathcal{S}}^{\prime}\), as described in (10). In (24), in the noise-free case, the solution is unique and easy to find. However, in the noisy environment, the theoretical unique solution to (24) is not guaranteed. Therefore, a key issue in estimating \(\theta\) is to find a value that satisfies a certain objective, i.e.,  
\[\widehat{\theta}_{R}\big{(}\mathbf{\mathcal{S}}^{*},\Delta\mathbf{\mathcal{S}}^{ \prime},\mathbf{D}^{\prime}\big{)}=\arg\min_{\theta\in[-\pi,\pi)}Obj\Big{(} \theta;\ \mathbf{\mathcal{S}}^{*},\Delta\mathbf{\mathcal{S}}^{\prime},\widehat{\mathbf{D}}^{ \prime}\Big{)}, \tag{36}\]  
where \(Obj\big{(}\theta;\ \mathbf{\mathcal{S}}^{*},\Delta\mathbf{\mathcal{S}}^{\prime}, \mathbf{D}^{\prime}\big{)}\) is an objective function of \(\theta\) given \(\mathbf{\mathcal{S}}^{*}\), \(\Delta\mathbf{\mathcal{S}}^{\prime}\), and \(\widehat{\mathbf{D}}^{\prime}\). In general, the least square estimator provides a good choice of objective function, as it is a well defined computationally efficient estimator.