## 2 Problem Statement  
Trilateration, and more generally, multilateration, is an important technique for 3D position estimation using distance measurements to four or more anchors with known position coordinates. Multilateration localization is used here to co-estimate the 3D positions of a set of fixed nodes and mobile agents (drones) capable of taking relative distance measurements in various scenarios. The first scenario consists in retrieving unknown 3D positions of a set of mobile agents given their relative distance measurements to a set of known anchors (Figure 1a). The second scenario (Figure 1b) consists of retrieving the 3D positions of a set of anchors using a set of mobile agents. In this case, an estimate of the positions of the mobile agents is assumed to be given using on-board sensors. The third scenario is a hybrid case where recovered positions of the anchors will improve the localization of moving agents. It deals with the problem of simultaneously obtaining an accurate estimation of the positions of drones and the active fixed sensors. This work gives a unified formulation to these scenarios, and solves the multilateration estimation problem with and without taking into account noisy range measurements and location uncertainties. In the following, each agent or active sensor node \(i\) of unknown position will be referred to as \(\mathbf{x}_{i}=(x_{i},y_{i},z_{i})^{T}\), denoting its 3D coordinates in a global reference frame and having \(d_{i,j}\) distance measurements to \(k\geq 4\) neighboring nodes with known position coordinates, denoted as \(\mathbf{x}_{\mathbf{n}j}=(x_{j},y_{j},z_{j})^{T},1\leq j\leq k\). Position/distance constraints can be formulated by (1) expressing that any node \(i\) of unknown an 3D position, \(\mathbf{x}_{i}=(x_{i},y_{i},z_{i})^{T}\), lies at the intersection of \(k\geq 4\) circles centered on the detected neighboring nodes with known positions and having the estimated range \(d_{i,j}\) as their radii:  
\[ \begin{pmatrix}(x_{i}-x_{1})^{2}+(y_{i}-y_{1})^{2}+(z_{i}-z_{1})^{2}\\ (x_{i}-x_{2})^{2}+(y_{i}-y_{2})^{2}+(z_{i}-z_{2})^{2}\\ \vdots\\ (x_{i}-x_{k})^{2}+(y_{i}-y_{k})^{2}+(z_{i}-z_{k})^{2}\end{pmatrix} = \begin{pmatrix} d_{i1}^{2}\\ d_{i2}^{2}\\ \vdots\\ d_{i,k}^{2}\end{pmatrix}  \tag{1}\]  
This leads to a nonlinear least squares problem that can have multiple local minima. The authors [34] have proposed a solution to the convex relaxation of this problem, but there will still be no guarantee that the solution will be optimal. Another way to solve this problem is to consider a linear solver by forming differences between pairs of these equations leading to the deletion of quadratic terms in (1). For instance, by subtracting the last component \(||\mathbf{x}_{i}-\mathbf{x}_{k}||^{2}\) from all others in (1), the problem (1) can then be rearranged and reformulated as follows:  
\[\mathbf{A}_{(k-1)\times 3}\;\mathbf{x}_{i}=\mathbf{D}_{(k-1)\times 1} \tag{2}\]  
where  
\[\mathbf{A}= \begin{pmatrix}(x_{k}-x_{1})&(y_{k}-y_{1})&(x_{k}-z_{1})\\ (x_{k}-x_{2})&(y_{k}-y_{2})&(x_{k}-z_{2})\\ \vdots&\\ (x_{k}-x_{k-1})&(y_{k}-y_{k-1})&(x_{k}-z_{k-1})\end{pmatrix} ;\mathbf{D}= \begin{pmatrix}d_{i1}^{2}-d_{i2}^{2}+x_{k}^{2}-x_{1}^{2}+y_{k}^{2}-y_{1}^{2}+z _{k}^{2}-z_{1}^{2}\\ d_{i2}^{2}-d_{i2}^{2}+x_{k}^{2}-x_{2}^{2}+y_{k}^{2}-y_{2}^{2}+z_{k}^{2}-z_{k}^{2 }\\ \vdots\\ d_{i,k-1}^{2}-d_{i,k}^{2}+x_{k}^{2}-x_{k-1}^{2}+y_{k}^{2}-y_{k-1}^{2}+z_{k}^{2 }-z_{k-1}^{2}\end{pmatrix} \]  
Note that the matrices \(\mathbf{A}\) and \(\mathbf{D}\) are expressed only as a function of neighboring nodes whose positions and relative distance measurements are known. Several methods have been proposed to solve the estimation problem (2). It could be solved using either the least squares approximation or other algebraic methods since, in the general case, the matrix \(\mathbf{A}\) is not necessarily square (depending on the number of neighboring nodes with known positions). This provides an exact solution if it exists and an approximate solution otherwise.