## 3 System Design
### UWB Localization
#### 3.2.2 Nonlinear Least Squares for 3D Position Estimation  
The proposed framework utilizes trilateration, a distance-based technique, for the positioning operations. Trilateration is a technique to determine an unknown point using distance information from three or more known points. In general, n + 1 anchor information is required to perform n-dimensional positioning. By solving the resulting system of nonlinear equations, the target's position can be effectively estimated. Figure 3: Diagram of asymmetric DS-TWR. One of the core strengths of trilateration lies in its simplicity and efficiency. When the distances from the anchors are precisely known, and the anchors are non-collinear, trilateration yields highly accurate positional estimates. Additionally, it is scalable and adaptable, making it a preferred choice for applications ranging from indoor positioning systems to Global Navigation Satellite Systems (GNSSs). In the context of 3D positioning, the process becomes somewhat intricate due to the introduction of a third dimension. We employed a nonlinear least-squares approach to enhance the accuracy and robustness of positional estimates, even in the presence of measurement noise and errors. In the proposed framework, the conventional trilateration technique is augmented through an algorithm that harnesses the power of nonlinear least-squares optimization, particularly the Levenberg-Marquardt algorithm [58], for enhanced accuracy and reliability in 3D position estimation. This enhancement is pivotal in scenarios where conventional trilateration might be susceptible to errors induced by various factors, including distance measurement inaccuracies and environmental noise. Algorithm 1 shows the detailed process of the trilateration technique we applied. The algorithm is initiated by defining a cost function \(f(\mathbf{p})\), which is the summation of the squared differences between the actual measured distances and the computed Euclidean distances from the estimated tag position to the anchors:  
\[f(\mathbf{p})=\sum_{i=1}^{4}\biggl{(}\sqrt{(\mathbf{a}_{i}-\mathbf{p})^{\top} (\mathbf{a}_{i}-\mathbf{p})}-d_{i}\biggr{)}^{2}. \tag{3}\]  
This cost function serves as the objective to be minimized, facilitating an accurate estimation of the tag's 3D coordinates \(\mathbf{p}\). ```
Input: Anchor positions \(\mathbf{A}=\{\mathbf{a}_{1},\mathbf{a}_{2},\mathbf{a}_{3},\mathbf{a}_{4}\}\), distances \(d=\{d_{1},d_{2},d_{3},d_{4}\}\) Output: Tag position \(\mathbf{p}\) and residual \(\mathbf{r}\) Function Main: Data: Define the cost function \(f(\mathbf{p})\) as the sum of squared differences between the measured distances and the distances computed from the estimated tag position: \(f(\mathbf{p})=\sum_{i=1}^{4}\Bigl{(}\sqrt{(\mathbf{a}_{i}-\mathbf{p})^{\top} (\mathbf{a}_{i}-\mathbf{p})}-d_{i}\Bigr{)}^{2}\) Data: Define the Jacobian matrix \(\mathbf{J}(\mathbf{p})\) of partial derivatives of \(f\) with respect to \(\mathbf{p}\): \(\mathbf{J}(\mathbf{p})=\Bigl{[}\frac{\partial f}{\partial p_{x}},\frac{ \partial f}{\partial p_{y}},\frac{\partial f}{\partial p_{z}}\Bigr{]}\) Choose an initial guess \(\mathbf{p}_{0}\) for the tag position Data: Set optimization settings: maximum iterations, tolerance, etc. Solve the nonlinear least-squares problem using Levenberg-Marquardt optimization algorithm to find \(\mathbf{p}=\arg\min_{\mathbf{p}}f(\mathbf{p})\) ifoptimization convergesthen Data: Compute the residual \(r(\mathbf{p})\) to evaluate the quality of the solution: \(r(\mathbf{p})=\sum_{i=1}^{4}\Bigl{(}\sqrt{(\mathbf{a}_{i}-\mathbf{p})^{\top} (\mathbf{a}_{i}-\mathbf{p})}-d_{i}\Bigr{)}^{2}\) Result: Return \(\mathbf{p}\) and \(\mathbf{r}\)  end if else Result: Return an error indication  end if
```  
**Algorithm 1** The 3D trilateration using nonlinear least squares. The optimization process employs the Jacobian matrix \(\mathbf{J}(\mathbf{p})\), consisting of the partial derivatives of the cost function with respect to the coordinates of the estimated tag position.