Cooperative Localization Using Distance Measurements for Mobile Nodes

Abstract

This paper considers the two-dimensional (2D) anchorless localization problem for sensor networks in global positioning system (GPS)-denied environments. We present an efficient method, based on the multidimensional scaling (MDS) algorithm, in order to estimate the positions of the nodes in the network using measurements of the inter-node distances. The proposed method takes advantage of the mobility of the nodes to address the location ambiguity problem, i.e., rotation and flip ambiguity, which arises in the anchorless MDS algorithm. Knowledge of the displacement of the moving node is used to produce an analytical solution for the noise-free case. Subsequently, a least squares estimator is presented for the noisy scenario and the associated closed-form solution derived. The simulations show that the proposed algorithm accurately and efficiently estimates the locations of nodes, outperforming alternative methods.

1. Introduction

Because of the increased availability of low-cost low-power sensors, smart sensors, and multi-functional sensors, wireless sensor networks are becoming increasingly ubiquitous [1,2,3,4]. Wireless sensor networks are being utilized in a diverse array of tracking and monitoring applications from environmental [5] and health monitoring [6] to traffic [7] and border surveillance [8]. In many of these applications, the nodes in the network are mobile and knowledge of their positions is a prerequisite for completing the task, and crucial for information sharing, data collection, and scheduling [9]. For example, if the locations of the nodes are unknown or significantly incorrect, the data they have collected from surrounding environment, such as wildlife [10] or weather information [11], will be useless, since the positional information is not available.

Localization algorithms estimate the locations of unknown nodes in the network using the positions of a known subset of the nodes to provide the required positional information. The most widely used localization techniques in the literature are distance-based localization algorithms, such as trilateration, radio interference positioning system (RIPS) [12], and the Hop-Distance algorithm [13]. These algorithms estimate the inter-node distances and require anchors, which is nodes with known locations, to provide the locations of the remaining nodes. The location of the anchor nodes is accessed via global positioning system (GPS) or a priori information [12,14,15,16,17,18].

While, GPS is widely used in locating unknown nodes in many situations, such as indoor, urban, and forest environments, the positions of nodes are difficult to obtain from GPS [19,20,21,22]. In this case, the anchors are absent or the positions of the anchors are not available, and the above algorithms cannot be applied. This is widely regarded as the most significant challenge in the positioning and navigation field [23,24,25]. Therefore, there is an increasing need for anchorless localization of sensor networks for use in GPS-denied or contested environments. Using the movement of nodes and the inter-node communication, cooperative localization can be leveraged in order to solve this problem. This scenario arises in the field of robotics swarms, especially in the navigation and formation control of unmanned aerial vehicles swarms under a GPS-denied environment [20,25,26,27].

In practice, cooperative localization can be achieved by utilizing the inter-node distances [28]. However, relative localization only gives node positions that satisfy the distance constraint, which means that there could be ambiguity problems, i.e., ambiguity due to rotation and/or flip [29]. The multidimensional scaling (MDS) algorithm is a widely used algorithm that is capable of tackling the anchorless localization problem [30,31,32,33,34,35]. The aim of MDS is to represent the similarity (or dissimilarity) of high dimensional data in a lower dimensional map which describes the relative distances between pairs of objects (in this case sensor nodes). Like other relative localization methods MDS can also be subject to the ambiguity problem; hence, algorithms have been proposed to attempt to address this problem. In [36] a MDS-based algorithm using moving nodes is presented, which constructs a cost function involving velocities and inter-node distances of all nodes at two consecutive time instants. In [37], a similar algorithm is proposed to solve the anchorless localization problem for nodes that can estimate the position via a nonlinear least square estimator; however, in this case, only one node is moving.

In this paper, we present an efficient algorithm for anchorless cooperative localization that is based on MDS. The algorithm mitigates the rotation and flip problems by taking advantage of the movement and inter-node communication of the mobile nodes. Unlike existing algorithms that operate in an iterative manner the proposed algorithm presents a closed-form solution that is computationally efficient. The algorithm is first derived in the noise-free case and the theoretical result is given. Subsrquently, the noisy case is considered and the associated closed-form estimator is presented. The proposed algorithm is supported by a rigorous theoretical derivation which provides optimal parameters. The simulation results support the theoretical analysis and indicate that the algorithm outperforms alternative methods.

This paper is organized, as follows: Section 2 introduces the background to the MDS algorithm and the associated ambiguity problem. The theoretical solution to the ambiguity problem is then presented in Section 3. Based on the theoretical analysis, Section 4 describes the impact of noise and introduces the proposed closed-form estimator for determining positions in noisy scenarios. Section 5 presents simulations for validating the proposed algorithm and, finally, Section 6 concludes the paper.

2. Multidimensional Scaling Algorithm

In this section, we briefly introduce the MDS algorithm and the ambiguity problem in an ideal scenario. As stated previously, the goal of MDS is to find a representation of the data that provides a low dimensional map (usually two or three dimensions) of the relative positions of the nodes based on their pairwise distances. If we consider the two-dimensional case where there are n nodes with their true coordinates denoted by ${\mathcal{S}}_i={[x_i,y_i]}^T$ where $i=1,\dots ,n$ and $n\ge 3$. Afterwards, in the noise-free case the distance between nodes i and j, where $i,j=1,\dots ,n$ and $i\ne j$ is given by $d_{i,j}$. If we assume in the ideal scenario that the nodes are able to measure the true distances between each other so that the pairwise distance between two nodes with coordinates ${\mathcal{S}}_i$ and ${\mathcal{S}}_j$ is given by

$$d_{i,j}=\parallel {\mathcal{S}}_i-{\mathcal{S}}_j\parallel =\sqrt{{({\mathcal{S}}_i-{\mathcal{S}}_j)}^T({\mathcal{S}}_i-{\mathcal{S}}_j)},$$

and, furthermore, the squared distance $d_{i,j}^2$ can be written as

$$d_{i,j}^2={\mathcal{S}}_i^T{\mathcal{S}}_i-2{\mathcal{S}}_i^T{\mathcal{S}}_j+{\mathcal{S}}_j^T{\mathcal{S}}_j,$$

then we have the following symmetric Euclidean distance matrix:

$$\mathbf{D}=\left[\begin{array}{ccccc}0& d_{1,2}^2& d_{1,3}^2& \cdots & d_{1,n}^2\\ d_{2,1}^2& 0& d_{2,3}^2& \cdots & d_{2,n}^2\\ d_{3,1}^2& d_{3,2}^2& 0& \cdots & d_{3,n}^2\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ d_{n,1}^2& d_{n,2}^2& d_{n,3}^2& \cdots & 0\end{array}\right].$$

(1)

If $\mathcal{S}=[{\mathcal{S}}_1,\cdots ,{\mathcal{S}}_n]$ is the collection of all of the node coordinates and $\psi$ is the diagonal elements of ${\mathcal{S}}^T\mathcal{S}$, i.e.,

$$\psi =\mathrm{diag}\left({\mathcal{S}}^T\mathcal{S}\right)={\left[{\mathbf{s}}_1^T{\mathbf{s}}_1,\dots ,{\mathbf{s}}_n^T{\mathbf{s}}_n\right]}^T,$$

then we can rewrite $\mathbf{D}$ as

$$\mathbf{D}=\psi {\mathbf{e}}^T-2{\mathcal{S}}^T\mathcal{S}+\mathbf{e}{\psi }^T,$$

(2)

where $\mathbf{e}={\left[1,\dots ,1\right]}^T$ is the vector of ones of length n. Using the centering operation $\mathbf{H}=\mathbf{I}-\mathbf{e}{\mathbf{e}}^T/n$, then we have

$$-\frac{1}{2}\mathbf{H}\mathbf{D}\mathbf{H}=\mathbf{U}\mathsf{\Lambda }{\mathbf{U}}^T.$$

where $\mathbf{U}\mathsf{\Lambda }{\mathbf{U}}^T$ is the eigendecomposition of the symmetric matrix $-\frac{1}{2}\mathbf{H}\mathbf{D}\mathbf{H}$. Then we can recover $\mathcal{S}$ (up to a translation and orthogonal transformation) via the following formula

$${\mathcal{S}}^{*}={\mathsf{\Lambda }}^{\frac{1}{2}}{\mathbf{U}}^T,$$

(3)

MDS is an efficient algorithm for resolving the relative positions of the nodes [32]. However, as is apparent from the above analysis, in the absence of anchor nodes, MDS can only give relative positions of the nodes, which can include rotation and flip ambiguity. In other words, the result of MDS maintains the relative inter-node distances; however, these calculated locations of the nodes may be flipped and/or rotated versions of the true positions of the nodes. Obviously, when considering navigation of mobile nodes or formation control of the sensor network, incorrect positions of the nodes can lead to problems.

The Ambiguity Problem

In order to consider the ambiguity problem, we assume a set of n nodes in 2D Euclidean space. We fix a coordinate system, which, without a loss of generality, places the first node ${\mathbf{s}}_1$ at the origin: ${\mathbf{s}}_1={[0,0]}^T$. We recall that knowledge of the distances provides an ambiguity up to a universal Euclidean transformation of the nodes. This fixing of node 1 at the origin removes the shift from this Euclidean transformation. Accordingly, the solution of the MDS, ${\mathcal{S}}^{*}$, is replaced by subtracting ${\mathbf{s}}_1^{*}$ from each column, so that, with some abuse of notation, ${\mathbf{s}}_1^{*}={[0,0]}^T$, and the other ${\mathbf{s}}_j^{*}$ become ${\mathbf{s}}_j^{*}-{\mathbf{s}}_1^{*}$. Once the shift is removed, the remaining ambiguity devolves to a rotation and a reflection (flip). We give the definitions of rotation and flip ambiguities, as follows.

Definition 1

(Rotation ambiguity). If there exists an angle $\theta \ne 2k\pi$, $k\in \mathbb{Z}$ such that

$$\mathcal{S}=\mathbf{M}\left(\theta \right){\mathcal{S}}^{*},$$

(4)

where $\mathbf{M}\left(\theta \right)$ is rotation matrix with angle θ and it is defined by [38]

$$\mathbf{M}\left(\theta \right)=\left[\begin{array}{cc}cos\left(\theta \right)& sin\left(\theta \right)\\ -sin\left(\theta \right)& cos\left(\theta \right)\end{array}\right],$$

(5)

then rotation ambiguity occurs.

Definition 2

(Flip ambiguity). Flip ambiguity occurs if

$$\mathcal{S}=\mathbf{F}{\mathcal{S}}^{*}\mathrm{with}\mathbf{F}=\left[\begin{array}{cc}-1& 0\\ 0& 1\end{array}\right].$$

(6)

Remark 1.

The matrix $\mathbf{F}$ can be defined equivalently by $\mathbf{F}=\left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right].$ This definition can be obtained via simply rotating (6) by π. In the following analysis, we use the definition of $\mathbf{F}$ as in Definition 2.

It can be seen that rotation ambiguity and flip ambiguity can occur simultaneously. If this is the case, then the true positions can be represented by

$$\mathcal{S}=\mathbf{M}\left(\theta \right)\mathbf{F}{\mathcal{S}}^{*}.$$

(7)

Figure 1 shows samples of these two ambiguities.

3. Resolving Rotation and Flip Ambiguities

In this section, the rotation ambiguity is analyzed mathematically in the noise-free scenario and an analytical solution to the rotation and flip ambiguities presented.

3.1. Analysis of Rotation Ambiguity

Firstly, we assume that there only exists rotation ambiguity between $\mathcal{S}$ and ${\mathcal{S}}^{*}$, no flip ambiguity. This means that based on the coordination rotation principle [38], ${\mathcal{S}}^{*}$ can be rotated to $\mathcal{S}$ using an unknown angle $\theta$ via (4). Hence, the true (unknown) locations of the nodes ${[x_i,y_i]}^T$, for $i=1,\dots ,n$, can be obtained by rotating ${[x_i^{*},y_i^{*}]}^T$ by $\theta$, i.e.,

$$\begin{array}{c}\hfill \mathcal{S}=\mathbf{M}\left(\theta \right){\mathcal{S}}^{*}.\end{array}$$

(8)

Therefore, we need to know $\theta$ to obtain the true locations of the nodes. In order to achieve this, we allow the lead node to move; this movement can then be utilized to obtain information that can be used to recover the locations of other nodes. Consider mobile nodes that are equipped with inertial navigation systems, the lead node can move with known displacement and orientation; that is, we let the lead node move to a known position, i.e., ${[\Delta x_1^{\prime },\Delta y_1^{\prime }]}^T$. Subsequently, the coordinates of all nodes after moving can be obtained as

$$\begin{array}{cc}\hfill {\mathcal{S}}^{\prime }& =\mathcal{S}+\Delta {\mathcal{S}}^{\prime }\hfill \end{array}$$

(9)

where the i-th column of $\Delta {\mathcal{S}}^{\prime }$ is given by

$$\Delta {\mathcal{S}}_i^{\prime }=\left\{\begin{array}{cc}{[\Delta x_1^{\prime },\Delta y_1^{\prime }]}^T\hfill & i=1\hfill \\ {[0,0]}^T\hfill & i\ne 1\hfill \end{array}\right.,$$

(10)

Accordingly, after moving the distance between the ith and jth nodes is

$$d_{i,j}^{\prime }=\parallel {\mathcal{S}}_i^{\prime }-{\mathcal{S}}_j^{\prime }\parallel ,$$

where ${\mathcal{S}}_i^{\prime }$ is the ith column of ${\mathcal{S}}^{\prime }$. We can then update the distance matrix (2) with the entries $d_{i,j}^{{}^{\prime }2}$ to give

$$\begin{array}{cc}\hfill {\mathbf{D}}^{\prime }& ={\psi }^{\prime }{\mathbf{e}}^T-2{{\mathcal{S}}^{\prime }}^T{\mathcal{S}}^{\prime }+\mathbf{e}{{\psi }^{\prime }}^T,\hfill \end{array}$$

(11)

where, by considering ${\mathcal{S}}_1={[0,0]}^T$ and $\Delta {\mathcal{S}}_i^{\prime }={[0,0]}^T$ for $i=2,\dots ,n$,

$$\begin{array}{c}\hfill {\psi }^{\prime }=\mathrm{diag}\left({\mathcal{S}}^{\prime T}{\mathcal{S}}^{\prime }\right)=\mathrm{diag}\left({\mathcal{S}}^T\mathcal{S}\right)+\mathrm{diag}(\Delta {{\mathcal{S}}^{\prime }}^T\Delta {\mathcal{S}}^{\prime })\triangleq \psi +\Delta {\psi }^{\prime }\end{array}$$

(12)

Substituting (9) and (12) into the distance matrix (11) we have

$$\begin{array}{cc}\hfill {\mathbf{D}}^{\prime }& =(\psi +\Delta {\psi }^{\prime }){\mathbf{e}}^T-2{(\mathcal{S}+\Delta {\mathcal{S}}^{\prime })}^T(\mathcal{S}+\Delta {\mathcal{S}}^{\prime })+\mathbf{e}{(\psi +\Delta {\psi }^{\prime })}^T\hfill \\ \hfill & =(\psi {\mathbf{e}}^T-2{\mathcal{S}}^T\mathcal{S}+\mathbf{e}{\psi }^T)+(\Delta {\psi }^{\prime }{\mathbf{e}}^T-2\Delta {{\mathcal{S}}^{\prime }}^T\Delta {\mathcal{S}}^{\prime }+\mathbf{e}\Delta {{\psi }^{\prime }}^T)-2\Delta {{\mathcal{S}}^{\prime }}^T\mathcal{S}-2{\mathcal{S}}^T\Delta {\mathcal{S}}^{\prime }\hfill \\ \hfill & =\mathbf{D}+\Delta {\mathbf{D}}^{\prime }-2\Delta {{\mathcal{S}}^{\prime }}^T\mathcal{S}-2{\mathcal{S}}^T\Delta {\mathcal{S}}^{\prime },\hfill \end{array}$$

giving

$$\begin{array}{cc}\hfill \mathbf{0}& =\mathbf{D}-{\mathbf{D}}^{\prime }+\Delta {\mathbf{D}}^{\prime }-2\Delta {{\mathcal{S}}^{\prime }}^T\mathcal{S}-2{\mathcal{S}}^T\Delta {\mathcal{S}}^{\prime },\hfill \end{array}$$

(13)

where $\mathbf{0}$ is a zero matrix with dimensions $n\times n$. This equation describes the relationship between both the locations and distance matrices pre and post the lead node moving; this information can be used to obtain the angle of rotation $\theta$. If we break the analysis of (13) into three parts, then we have the following:

• Since only the lead node’s position is changed, then the term $\mathbf{D}-{\mathbf{D}}^{\prime }$ in (13) becomes

  $$\mathbf{D}-{\mathbf{D}}^{\prime }=\left[\begin{array}{cccc}0& d_{1,2}^2-d_{1,2}^{{}^{\prime }2}& \cdots & d_{1,n}^2-d_{1,n}^{{}^{\prime }2}\\ d_{1,2}^2-d_{1,2}^{{}^{\prime }2}& 0& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ d_{1,n}^2-d_{1,n}^{{}^{\prime }2}& 0& \cdots & 0\end{array}\right].$$

  (14)
• In (13), $\Delta \mathbf{D}$ is the distance matrix between point $\Delta {\mathcal{S}}_1$ and $n-1$ origin points ${[0,0]}^T$, i.e.,

  $$\Delta {\mathbf{D}}^{\prime }=\left[\begin{array}{cccc}0& \Delta {x_1^{\prime }}^2+\Delta {y_1^{\prime }}^2& \cdots & \Delta {x_1^{\prime }}^2+\Delta {y_1^{\prime }}^2\\ \Delta {x_1^{\prime }}^2+\Delta {y_1^{\prime }}^2& 0& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ \Delta {x_1^{\prime }}^2+\Delta {y_1^{\prime }}^2& 0& \cdots & 0\end{array}\right].$$

  (15)
• For the term $-2\Delta {{\mathcal{S}}^{\prime }}^T\mathcal{S}-2{\mathcal{S}}^T\Delta {\mathcal{S}}^{\prime }$ in (13), since ${\mathcal{S}}^T\Delta {\mathcal{S}}^{\prime }$ can be calculated by

  $${\mathcal{S}}^T\Delta {\mathcal{S}}^{\prime }=\left[\begin{array}{cc}0& 0\\ x_2& y_2\\ x_3& y_3\\ ⋮& ⋮\\ x_n& y_n\end{array}\right]\left[\begin{array}{ccccc}\Delta x_1^{\prime }& 0& 0& \cdots & 0\\ \Delta y_1^{\prime }& 0& 0& \cdots & 0\end{array}\right]=\left[\begin{array}{cccc}0& 0& \cdots & 0\\ x_2\Delta x_1^{\prime }+y_2\Delta y_1^{\prime }& 0& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ x_n\Delta x_1^{\prime }+y_n\Delta y_1^{\prime }& 0& \cdots & 0\end{array}\right],$$

  (16)

  therefore

  $$-\left(2\Delta {{\mathcal{S}}^{\prime }}^T\mathcal{S}+2{\mathcal{S}}^T\Delta {\mathcal{S}}^{\prime }\right)=-2\left({\left({\mathcal{S}}^T\Delta {\mathcal{S}}^{\prime }\right)}^T+{\mathcal{S}}^T\Delta {\mathcal{S}}^{\prime }\right).$$

  (17)
  Inserting the rotation (8) into $x_i\Delta x_1^{\prime }+y_i\Delta y_1^{\prime }$ in (16), for $i=2,\dots ,n$, we have

  $$\begin{array}{cc}\hfill x_i\Delta x_1^{\prime }+y_i\Delta y_1^{\prime }& =x_i^{*}\Delta x_1^{\prime }cos\left(\theta \right)+y_i^{*}\Delta x_1^{\prime }sin\left(\theta \right)+y_i^{*}\Delta y_1^{\prime }cos\left(\theta \right)-x_i^{*}\Delta y_1^{\prime }sin\left(\theta \right)\hfill \\ \hfill & =(x_i^{*}\Delta x_1^{\prime }+y_i^{*}\Delta y_1^{\prime })cos\left(\theta \right)+(y_i^{*}\Delta x_1^{\prime }-x_i^{*}\Delta y_1^{\prime })sin\left(\theta \right).\hfill \end{array}$$

  (18)
  Using (18) allows us to express (17) in a way that is independent of ${[x_i,y_i]}^T$.

If we combine (14)–(18), then (13) becomes

$$\mathbf{D}-{\mathbf{D}}^{\prime }+\Delta {\mathbf{D}}^{\prime }-2\Delta {{\mathcal{S}}^{\prime }}^T\mathcal{S}-2{\mathcal{S}}^T\Delta {\mathcal{S}}^{\prime }=\left[\begin{array}{cccc}0& f_2\left(\theta \right)& \cdots & f_n\left(\theta \right)\\ f_2\left(\theta \right)& 0& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ f_n\left(\theta \right)& 0& \cdots & 0\end{array}\right]=\mathbf{0},$$

(19)

where

$$\begin{array}{c}\hfill f_i\left(\theta \right)=a_i+b_{i}cos\left(\theta \right)+c_{i}sin\left(\theta \right),i=2,\dots ,n\end{array}$$

(20)

with coefficients

$$\begin{array}{cc}\hfill a_i& =d_{1,i}^2-d_{1,i}^{{}^{\prime }2}+\Delta {x_1^{\prime }}^2+\Delta {y_1^{\prime }}^2\hfill \end{array}$$

(21)

$$\begin{array}{cc}\hfill b_i& =-2(x_i^{*}\Delta x_1^{\prime }+y_i^{*}\Delta y_1^{\prime })\hfill \end{array}$$

(22)

$$\begin{array}{cc}\hfill c_i& =2(x_i^{*}\Delta y_1^{\prime }-y_i^{*}\Delta x_1^{\prime })\hfill \end{array}$$

(23)

Equation (19) is equivalent to the following system of equations:

$$\left\{\begin{array}{cc}{a}_2+b_{2}cos\left(\theta \right)+c_{2}sin\left(\theta \right)=\hfill & 0\hfill \\ ⋮\hfill & ⋮\hfill \\ a_n+b_{n}cos\left(\theta \right)+c_{n}sin\left(\theta \right)=\hfill & 0\hfill \end{array}\right.,$$

(24)

Finally, the solution to (24) is the angle to resolve the rotation ambiguity. Importantly, this solution can be shown to be unique when $n\ge 3$. If we consider the case of $n=3$, then (24) can be expressed as

$$\left\{\begin{array}{c}{\displaystyle sin\left(\theta \right)=\frac{a_3{b}_2-a_2{b}_3}{b_3{c}_2-b_2{c}_3}\triangleq W_1}\hfill \\ {\displaystyle cos\left(\theta \right)=\frac{a_2{c}_3-a_3{c}_2}{b_3{c}_2-b_2{c}_3}\triangleq W_2}\hfill \end{array}\right..$$

(25)

Obviously, given $W_1$ and $W_2$, (25) has a unique solution to $\theta$ within $\left[-\pi ,\pi \right)$. Similarly, it is straightforward to show that, when $n\ge 3$, (24) has a unique solution within $\left[-\pi ,\pi \right)$.

3.2. Analysis of Rotation and Flip Ambiguities

Having obtained a unique solution to the rotation angle when only rotation ambiguity is present, in this section we present an analytical solution that is based on the analysis in Section 3.1 for when rotation and flip ambiguities occur simultaneously. The key idea behind this method is again to use the mobility of the lead node to acquire extra information in order to detect the flip of the initial MDS localization result.

Firstly, we assume that there exist three non-collinear nodes in order to be able to detect flip ambiguity. Next, we note that the ith equation in (24) has the following solutions ${\theta }_{i,1}$ and ${\theta }_{i,2}$:

$${\theta }_{i,1,2}=atan2\left(a_i{b}_i\pm |c_i|\sqrt{b_i^2+c_i^2-a_i^2},a_i{c}_i\mp \frac{b_i}{c_i}\left|c_i\right|\sqrt{b_i^2+c_i^2-a_i^2}\right),$$

(26)

where $atan2(·,·)\in [-\pi ,\pi )$ is the 2-argument arctangent. Therefore, the solution to (26), which is common to all values of i, $\forall i=2,\dots ,n$ is the unique solution to (24). It can also be shown that ${\theta }_{i,1,2}\in [-\pi ,\pi )$ given in (26) can be rewritten and rearranged into a concise form

$$\begin{array}{cc}\hfill {\theta }_{i,1}& =g\left(atan2(y_i^{*},x_i^{*})-atan2(y_i,x_i)\right)\hfill \end{array}$$

(27)

$$\begin{array}{cc}\hfill {\theta }_{i,2}& =g\left({\theta }_{i,1}+2{\mathsf{\Theta }}_i\right)\hfill \end{array}$$

(28)

where ${\mathsf{\Theta }}_i=atan2\left(y_i,x_i\right)+atan2\left(\Delta x_1^{\prime },\Delta y_1^{\prime }\right)-\frac{\pi }{2}$ and the function $g\left(t\right)=t-2\pi ⌊\frac{t}{2\pi }+\frac{1}{2}⌋$ can wrap any arbitrary angle t in radians into range $[-\pi ,\pi )$. Appendix A provides the full derivation of these equations.

The angles ${\theta }_{i,1}$ in (27) represent the angles between vectors $\overrightarrow{{\mathcal{S}}_1{\mathcal{S}}_i^{*}}$ and $\overrightarrow{{\mathcal{S}}_1{\mathcal{S}}_i}$ for $i=2,\dots ,n$, as shown in Figure 2, and play an important role in the ability to detect flip ambiguity. When there is only rotation ambiguity, because of the uniqueness of solution of (24), we have ${\theta }_{i,1}={\theta }_{j,1}$, $\forall i,j=2,\dots ,n$, as shown in the previous section. Whereas, the angles ${\theta }_{i,2}$ in (28) are the summation of ${\theta }_{i,1}$ and the angle induced by $\Delta x_1^{\prime }$ and $\Delta x_2^{\prime }$. Obviously, if there exist three non-collinear nodes, ${\mathsf{\Theta }}_i\ne {\mathsf{\Theta }}_j$, $\forall i,j=2,\dots ,n$ and $i\ne j$ and, therefore, from (28) we have ${\theta }_{i,2}\ne {\theta }_{j,2}$, $\forall i,j=2,\dots ,n$. In contrast, it can be shown that flip ambiguity exists if and only if ${\theta }_{i,1}\ne {\theta }_{j,1}$, $\forall i,j=2,\dots ,n$ and $i\ne j$. In order to illustrate why this is the case, we give the counter example, assuming without loss of generality, that $n=3$, ${\theta }_{2,1}={\theta }_{3,1}$ and flip ambiguity exists. ${\theta }_{2,1}={\theta }_{3,1}$ implies that ${\mathcal{S}}_i^{*}$, $i=2,3$, can be rotated simultaneously to the true positions ${\mathcal{S}}_i$ via either ${\theta }_{2,1}$ or ${\theta }_{3,1}$, as shown in Figure 2a. Hence, this contradicts the assumption of the existence of flip ambiguity.

Furthermore, extending to the case where $n>3$, still assuming that ${\theta }_{2,1}={\theta }_{3,1}$ and flip ambiguity exists. We know that, for $i=2,3$, ${\mathcal{S}}_i^{*}$ can be simultaneously rotated to the true positions ${\mathcal{S}}_i$ via ${\theta }_{2,1}$ (or ${\theta }_{3,1}$). Because there exist three non-collinear nodes, then three nodes with correct positions are sufficient for guaranteeing the localization of the whole network [39]. In this case, ${\mathbf{s}}_1$, ${\mathbf{s}}_2$ and ${\mathbf{s}}_3$ can be found exactly from ${\theta }_{2,1}$ (or ${\theta }_{3,1}$). Therefore, ${\mathcal{S}}_i$, for $i=4,\dots ,n$, must be solvable via rotating ${\mathcal{S}}_i^{*}$ by angle ${\theta }_{2,1}$ (or ${\theta }_{3,1}$) and, as a result, ${\theta }_{i,1}={\theta }_{2,1}={\theta }_{3,1}$. This again contradicts the assumption of existence of flip ambiguity. On the other hand, if ${\theta }_{i,1}\ne {\theta }_{j,1}$, $\forall i,j=2,\cdots ,n$ and $i\ne j$, then it is obvious that there exists flip ambiguity, since ${\mathbf{s}}_i^{*}$ cannot be rotated to ${\mathcal{S}}_i$ simultaneously. As a conclusion, there exists flip ambiguity if and only if ${\theta }_{i,1}\ne {\theta }_{j,1}$, $\forall i,j=2,\cdots ,n$ and $i\ne j$.

In what follows, we assume that there exist three non-collinear nodes and, based on the above analysis, we make the following conclusion:

$$\forall i,j=2,\dots ,n\mathrm{and}i\ne j\left\{\begin{array}{cc}{\theta }_{i,1}={\theta }_{j,1}(\mathrm{equivalently}{\theta }_{i,2}\ne {\theta }_{j,2}),\hfill & \mathrm{If}\mathrm{no}\mathrm{flip}\mathrm{ambiguity}\mathrm{exists}.\hfill \\ {\theta }_{i,1}\ne {\theta }_{j,1}(\mathrm{equivalently}{\theta }_{i,2}={\theta }_{j,2}),\hfill & \mathrm{If}\mathrm{flip}\mathrm{ambiguity}\mathrm{exists}.\hfill \end{array}\right.$$

(29)

Although we cannot use (27)–(29) directly to determine the existence of flip as they contain unknown true positions, those results are crucial in deriving the estimator for locations in noisy scenario.

In reality, the solution to (24) is computed using ${\mathcal{S}}^{*}$, $\Delta {\mathcal{S}}^{\prime }$ and ${\mathbf{D}}^{\prime }$. We denote this unique solution by ${\theta }_R\left({\mathcal{S}}^{*},\Delta {\mathcal{S}}^{\prime },{\mathbf{D}}^{\prime }\right)$. The same notation ${\theta }_R(·,·,·)$ is used to denote a rotation angle that is calculated with different variables, nonetheless, whatever the variables used, the method is the same as described above. If we denote the positions that are calculated using the rotation angle ${\theta }_R\left({\mathcal{S}}^{*},\Delta {\mathcal{S}}^{\prime },{\mathbf{D}}^{\prime }\right)$ as ${\mathcal{S}}^{**}$ we have the following result

$${\mathcal{S}}^{**}=\mathbf{M}\left({\theta }_R({\mathcal{S}}^{*},\Delta {\mathcal{S}}^{\prime },{\mathbf{D}}^{\prime })\right){\mathcal{S}}^{*}=\left[\begin{array}{ccc}{x}_1^{**},& x_2^{**},& \cdots ,x_n^{**}\\ y_1^{**},& y_2^{**},& \cdots ,y_n^{**}\end{array}\right].$$

(30)

If the lead node then moves to a second position $[\Delta x_1^{″},\Delta y_1^{″}]$ giving a matrix formed by true positions ${\mathcal{S}}^{″}=\mathcal{S}+\Delta {\mathcal{S}}^{″}$, where

$$\Delta {\mathcal{S}}^{″}=\left[\Delta {\mathcal{S}}_1^{″},\Delta {\mathcal{S}}_2^{″},\dots ,\Delta {\mathcal{S}}_n^{″}\right]=\left[\begin{array}{cccc}\Delta x_1^{″}& 0& \cdots & 0\\ \Delta y_1^{″}& 0& \cdots & 0\end{array}\right].$$

(31)

After obtaining a new distance matrix ${\mathbf{D}}^{″}$ at position ${\mathcal{S}}^{″}$, we can solve ${\theta }_R({\mathcal{S}}^{**},\Delta {\mathcal{S}}^{″},{\mathbf{D}}^{″})$. If ${\theta }_R({\mathcal{S}}^{**},\Delta {\mathcal{S}}^{″},{\mathbf{D}}^{″})=0$, then there is no flip ambiguity and the true positions are $\mathcal{S}={\mathcal{S}}^{**}$; otherwise, we move to the process of resolving the flip ambiguity. For this, according to Definition 2, all the values in ${\mathcal{S}}^{*}$ along the $x-$axis are required to be flipped to obtain $\mathbf{F}{\mathcal{S}}^{*}$. Subsequently, we only need to calculate the rotation angle using ${\theta }_R(\mathbf{F}{\mathcal{S}}^{*},\Delta {\mathcal{S}}^{\prime },{\mathbf{D}}^{\prime })$. It should be noted that, since $\Delta {\mathcal{S}}^{\prime }$ and ${\mathbf{D}}^{\prime }$ are fixed, it is unnecessary to take any new measurements and the true position $\mathcal{S}$ can be resolved by

$$\mathcal{S}=\mathbf{M}\left({\theta }_R(\mathbf{F}{\mathcal{S}}^{*},\Delta {\mathcal{S}}^{\prime },{\mathbf{D}}^{\prime })\right)\mathbf{F}{\mathcal{S}}^{*}.$$

(32)

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 ith and jth 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 ${\overline{d}}_{i,j}$ and ${\overline{d}}_{j,i}$, then ${\overline{d}}_{i,j}\ne {\overline{d}}_{j,i}$ resulting in uncertainty in our estimates of the distances. In general, the measured distance between the ith and jth nodes can be modeled by

$${\overline{d}}_{i,j}=d_{i,j}+{\omega }_{i,j},i,j=1,\dots ,n,i\ne j,$$

(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

$$\overline{\mathbf{D}}=\left[\begin{array}{ccccc}0& {\overline{d}}_{1,2}^2& {\overline{d}}_{1,3}^2& \cdots & {\overline{d}}_{1,n}^2\\ {\overline{d}}_{2,1}^2& 0& {\overline{d}}_{2,3}^2& \cdots & {\overline{d}}_{2,n}^2\\ {\overline{d}}_{3,1}^2& {\overline{d}}_{3,2}^2& 0& \cdots & {\overline{d}}_{3,n}^2\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ {\overline{d}}_{n,1}^2& {\overline{d}}_{n,2}^2& {\overline{d}}_{n,3}^2& \cdots & 0\end{array}\right].$$

(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

$${\widehat{d}}_{i,j}={\widehat{d}}_{j,i}=\mathbb{E}({\overline{d}}_{i,j},{\overline{d}}_{j,i})$$

(35)

where $\mathbb{E}({\overline{d}}_{i,j},{\overline{d}}_{j,i})$ is an estimator for the distance between i-th and j-th nodes using ${\overline{d}}_{i,j}$ and ${\overline{d}}_{j,i}$. In practice, this estimator is designed by using the knowledge of noise distribution. For example, when the noise in ${\overline{d}}_{i,j}$ and ${\overline{d}}_{j,i}$ are assumed to comprise independent and identically distributed (i.i.d.) normal distributions, then we have ${\widehat{d}}_{i,j}={\widehat{d}}_{j,i}=\frac{1}{2}\left({\overline{d}}_{i,j}+{\overline{d}}_{j,i}\right)$ [40]. Because the study of the estimator $\mathbb{E}({\overline{d}}_{i,j},{\overline{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 ${\widehat{d}}_{i,j}$. Similarly, we can obtain ${\widehat{\mathbf{D}}}^{\prime }$ by using the new position of ${\mathcal{S}}^{\prime }=\mathcal{S}+\Delta {\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\left({\mathcal{S}}^{*},\Delta {\mathcal{S}}^{\prime },{\mathbf{D}}^{\prime }\right)=arg\underset{\theta \in [-\pi ,\pi )}{min}Obj\left(\theta ;{\mathcal{S}}^{*},\Delta {\mathcal{S}}^{\prime },{\widehat{\mathbf{D}}}^{\prime }\right),$$

(36)

where $Obj\left(\theta ;{\mathcal{S}}^{*},\Delta {\mathcal{S}}^{\prime },{\mathbf{D}}^{\prime }\right)$ is an objective function of $\theta$ given ${\mathcal{S}}^{*}$, $\Delta {\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. The objective function based on the least square estimator is given by

$$Obj\left(\theta ;{\mathcal{S}}^{*},\Delta {\mathcal{S}}^{\prime },{\mathbf{D}}^{\prime }\right)=\sum _{i=1}^n{\left(a_i+b_{i}cos\left(\theta \right)+c_{i}sin\left(\theta \right)\right)}^2.$$

(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

$${\theta }_{\lambda }=\left\{arcsin\left({\lambda }_1\right),g(\pi -arcsin\left({\lambda }_1\right)),\dots ,arcsin\left({\lambda }_m\right),g(\pi -arcsin\left({\lambda }_m\right))\right\}\in [-\pi ,\pi ),$$

where ${\lambda }_j\in [-1,1]$, $j=1,\dots ,m$, and m is the number of solutions, such that $1\le m\le 4$. Subsequently, (37) becomes

$${\widehat{\theta }}_R\left({\mathcal{S}}^{*},\Delta {\mathcal{S}}^{\prime },{\widehat{\mathbf{D}}}^{\prime }\right)=arg\underset{\theta \in {\mathbf{\theta }}_{\lambda }}{min}Obj\left(\theta ;{\mathcal{S}}^{*},\Delta {\mathcal{S}}^{\prime },{\widehat{\mathbf{D}}}^{\prime }\right),$$

(38)

and, accordingly, by (30)

$${\mathcal{S}}^{**}=\mathbf{M}\left({\widehat{\theta }}_R\left({\mathcal{S}}^{*},\Delta {\mathcal{S}}^{\prime },{\widehat{\mathbf{D}}}^{\prime }\right)\right){\mathcal{S}}^{*}.$$

(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^{″},\Delta y_1^{″}]}^T$ and solve ${\widehat{\theta }}_R({\mathcal{S}}^{**},$$\Delta {\mathcal{S}}^{″},{\widehat{\mathbf{D}}}^{″})$ via (38) while using the estimated distance matrix ${\widehat{\mathbf{D}}}^{″}$ obtained at the new position ${\mathcal{S}}^{″}=\mathcal{S}+\Delta {\mathcal{S}}^{″}$. However, becasue of the presence of noise ${\widehat{\theta }}_R\left({\mathcal{S}}^{**},\Delta {\mathcal{S}}^{″},{\widehat{\mathbf{D}}}^{″}\right)$ 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:

$$\left\{\begin{array}{cc}\mathrm{no}\mathrm{flip},\hfill & \mathrm{if}\left|{\widehat{\theta }}_R\left({\mathcal{S}}^{**},\Delta {\mathcal{S}}^{″},{\widehat{\mathbf{D}}}^{″}\right)\right|\le \left|l\right|\hfill \\ \mathrm{flip},\hfill & \mathrm{if}\left|{\widehat{\theta }}_R\left({\mathcal{S}}^{**},\Delta {\mathcal{S}}^{″},{\widehat{\mathbf{D}}}^{″}\right)\right|>\left|l\right|,\hfill \end{array}\right.$$

(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\left({\mathcal{S}}^{**},\Delta {\mathcal{S}}^{″},{\mathbf{D}}^{″}\right)=g\left(-2atan2(\Delta x_1^{\prime },\Delta y_1^{\prime })+2atan2(\Delta x_1^{″},\Delta y_1^{″})\right),$$

(41)

where $g\left(t\right)=t-2\pi ⌊\frac{t}{2\pi }+\frac{1}{2}⌋$.

From Proposition 1, under the noise-free case, we know the value of ${\theta }_R\left({\mathcal{S}}^{**},\Delta {\mathcal{S}}^{″},{\mathbf{D}}^{″}\right)$ when a flip occurs; therefore, in the noisy case, the optimal threshold is

$$l=\frac{1}{2}g\left(-2atan2(\Delta x_1^{\prime },\Delta y_1^{\prime })+2atan2(\Delta x_1^{″},\Delta y_1^{″})\right).$$

As a conclusion, the positions of nodes can estimated using the following formula:

$$\widehat{\mathcal{S}}=\left\{\begin{array}{cc}\mathbf{M}\left({\widehat{\theta }}_R\left({\mathcal{S}}^{*},\Delta {\mathcal{S}}^{\prime },{\widehat{\mathbf{D}}}^{\prime }\right)\right){\mathcal{S}}^{*},\hfill & \mathrm{if}\left|{\widehat{\theta }}_R\left({\mathcal{S}}^{**},\Delta {\mathcal{S}}^{″},{\widehat{\mathbf{D}}}^{″}\right)\right|\le \left|l\right|\hfill \\ \mathbf{M}\left({\widehat{\theta }}_R\left(\mathbf{F}{\mathcal{S}}^{*},\Delta {\mathcal{S}}^{\prime },{\widehat{\mathbf{D}}}^{\prime }\right)\right)\mathbf{F}{\mathcal{S}}^{*},\hfill & \mathrm{if}\left|{\widehat{\theta }}_R\left({\mathcal{S}}^{**},\Delta {\mathcal{S}}^{″},{\widehat{\mathbf{D}}}^{″}\right)\right|>\left|l\right|\hfill \end{array}\right.$$

(42)

Algorithm 1 summarizes the algorithm to estimate locations of nodes with noisy measurements.

Algorithm 1: Algorithm to estimate locations of mobile nodes.

5. Simulations

Networks with randomly generated nodes were used to test the performance of the proposed algorithm. In all of the following simulations the position of the first node is fixed to $[0,0]$ and the positions of the other nodes are uniformly generated within $[-20,20]$.

To validate the proposed approach that can address the ambiguity problem, we use the same noise-free scenarios as given in Figure 1 the results of which are shown in Figure 3. Unlike the results that are obtained from the MDS alone, as shown in Figure 1, when we compare the locations of the true and estimated nodes in Figure 3, we can see that, while using the method that is presented in Section 3, the rotation and flip ambiguities are solved correctly and, therefore, the positions of the nodes are successfully recovered.

Next, we consider the noisy scenario, as discussed in Section 4, in the noisy scenario the detection of whether flip has occurred is no longer straightforward and requires an appropriate choice of threshold. Firstly, to demonstrate the ability of the proposed algorithm to detect flip or no flip states, we tested two different network configurations: one with the number of nodes $n=6$ and the other with $n=10$. The noise is assumed to be Gaussian distributed with mean 0 and standard deviation, $\sigma$, of $0.01$. For each of the configurations, the outcomes of 50 simulations are shown in Figure 4. The results show that the proposed algorithm can correctly detect flip ambiguity in the noisy scenario.

Having established that the proposed algorithm can successfully deal with the ambiguity problem, we now consider the performance of the algorithm in terms of the accuracy of the localization. For the sake of simplicity, we define the noise level, standard variance of the noise, as $\delta =-20{log}_{10}\sigma$, with $\delta$ chosen to range from 20 to 50 in steps of five and $\sigma$ calculated accordingly. For each noise level, the Root-Mean-Square Error (RMSE) from 2000 simulations is used to evaluate the performance of the localization. The RMSE is defined by

$$\begin{array}{c}\hfill \mathrm{RMSE}=\frac{1}{n}\sum _{i=1}^n\sqrt{\sum _{j=1}^m\frac{{({\widehat{x}}_{i,j}-x_{i,j})}^2+{({\widehat{y}}_{i,j}-y_{i,j})}^2}{m}}\end{array}$$

(43)

where $(x_{i,j},y_{i,j})$ and $({\widehat{x}}_{i,j},{\widehat{y}}_{i,j})$ are the true and estimated location of i-th, $i=1,\cdots ,n$, node in j-th, $j=1,\cdots ,m$, Monte Carlo simulation. It is assumed that $(x_{i,j},y_{i,j})$ is generated within region $[-100,100]\times [-100,100]$ uniformly in each Monte Carlo simulation.

In order to provide a comparison with the proposed algorithm, we implemented the nonlinear least squares estimator (NLSE) that is presented in [37], solving the nonlinear least square problem using an optimization method. Additionally, the proposed algorithm essentially takes advantage of the mobility of the node to create virtual anchors for localizing the unknown nodes. As a result, the alignment method of relative locations proposed in [30,42] can be potentially applied in this scenario. As a comparison, the performance of this conventional method is also given.

From the simulation results that are shown in Figure 5, it can be seen that the proposed algorithm has better performance in terms of the RMSE for different noise levels and numbers of nodes than the NLS estimator with the conventional method. The localization error are arise from two effects: 1. MDS localization error; 2. mis-alignment error, i.e., the error from inaccurately aligning the positions. Additionally, it should also be noted that the algorithm proposed in this paper can simultaneously estimate the positions of all of the nodes, which cannot be achieved using the algorithm that was presented in [37]. As an indication of the computational efficiency of the proposed algorithm, our simulations indicate a ratio of required CPU time of the proposed algorithm relative to NLSE is Proposed algorithm:NLSE = 1:8.83.

6. Conclusions

In this paper, we have presented an efficient cooperative localization algorithm that is based on MDS. The algorithm provides a practical solution for anchorless localization of mobile nodes using noisy measurements. Unlike traditional MDS algorithms, which suffer from an ambiguity problem, the algorithm that is presented here can solve the flip and rotation ambiguities and accurately estimate the positions of nodes in 2D space. The simulation results demonstrate the accuracy of the algorithm, showing that it outperforms alternative methods. At the same time, the proposed algorithm provides greater efficiency than alternative solutions that operate in an iterative manner by providing a closed-form solution. We point out that the main limitations of this algorithm are twofold. Firstly, as mentioned above, this algorithm has been developed in a 2D scenario, which limits its application in more complex situations. Although one may follow a similar procedure to derive the corresponding algorithm for a more general case, i.e., 3D space, this is non-trivial, as the geometry of the network has more degrees of freedom in the 3D space. Secondly, and in a similar vein to other algorithms, the algorithm proposed here requires inertial navigation to provide the displacement of the moving node. Consequently, a deeper analysis of the error that arises from the inertial navigation system should be taken into account in improving this algorithm. These issues will be addressed in future work.