3-D Relative Localization for Multi-Robot Systems with Angle and Self-Displacement Measurements

Abstract: Realizing relative localization by leveraging inter-robot local measurements is a challenging problem, especially in the presence of measurement noise. Motivated by this challenge, in this paper we propose a novel and systematic 3-D relative localization framework based on inter-robot interior angle and self-displacement measurements. Initially, we propose a linear relative localization theory comprising a distributed linear relative localization algorithm and sufficient conditions for localizability. According to this theory, robots can determine their neighbors' relative positions and orientations in a purely linear manner. Subsequently, in order to deal with measurement noise, we present an advanced Maximum a Posterior (MAP) estimator by addressing three primary challenges existing in the MAP estimator. Firstly, it is common to formulate the MAP problem as an optimization problem, whose inherent non-convexity can result in local optima. To address this issue, we reformulate the linear computation process of the linear relative localization algorithm as a Weighted Total Least Squares (WTLS) optimization problem on manifolds. The optimal solution of the WTLS problem is more accurate, which can then be used as initial values when solving the optimization problem associated with the MAP problem, thereby reducing the risk of falling into local optima. The second challenge is the lack of knowledge of the prior probability density of the robots' relative positions and orientations at the initial time, which is required as an input for the MAP estimator. To deal with it, we combine the WTLS with a Neural Density Estimator (NDE). Thirdly, to prevent the increasing size of the relative positions and orientations to be estimated as the robots continuously move when solving the MAP problem, a marginalization mechanism is designed, which ensures that the computational cost remains constant.

• The paper introduces a novel 3-D linear framework for relative localization using angle and self-displacement measurements in multi-robot systems.
• It leverages tetrahedral rigidity, weighted least squares initialization, and neural prior estimation to mitigate non-convexity and sensor noise.
• Empirical and real-world experiments confirm its superior accuracy, scalability, and robustness compared to traditional non-linear methods.

3-D Relative Localization for Multi-Robot Systems with Angle and Self-Displacement Measurements

Introduction and Motivation

Relative localization in multi-robot systems (MRS) is essential for enabling cooperative behaviors such as formation control, exploration, and collaborative perception. Accurate estimation of both positions and orientations among robots, particularly under locally available and noisy sensory data, presents significant technical challenges. Most existing frameworks hinge on nonlinear optimization, strict assumptions about prior densities, or a reliance on measurement modalities (e.g., distances or bearings) not always practical for generic platforms. This paper introduces a systematic 3-D relative localization framework that operates on interior angle and self-displacement measurements while explicitly addressing major limitations associated with traditional Maximum a Posteriori (MAP) estimators.

Linear Relative Localization Theory

The core theoretical contribution is a fully linear, algebraically grounded framework for relative localization based on inter-robot interior angle and self-displacement data. The authors formalize the information structure using tetrahedrally angle-rigid sets, extending geometric rigidity concepts to guarantee localizability in 3-D by constructing appropriate multi-robot topologies (Figure 1). Relative localizability is shown to be guaranteed when the communication and measurement structure can be decomposed into overlapping tetrahedra, for which the paper provides explicit sufficient conditions.

Localization computations are performed by solving linear equations induced by measured angles and self-displacements over a horizon of 2–3 time instants, depending on the relative alignment of robot frames. Crucially, this formulation obviates the need for iterative nonlinear solvers or the explicit handling of ambiguous solutions that typically arise with range- or bearing-only methods. The explicit handling of unaligned coordinate frames by jointly estimating both relative positions and local frame orientations makes the approach robust to the lack of global heading information—a pervasive real-world constraint.

Figure 2: Overview of the proposed relative localization framework, integrating linear estimation, WTLS-based initialization, NDE-driven prior learning, and MAP optimization.

Noise-Robust Estimation: MAP on Manifolds

To robustify the localization against measurement noise, the authors integrate and extend the MAP estimator. Their framework addresses three critical issues:

(1) Non-convexity Mitigation via WTLS Initialization.

Rather than direct optimization of the non-convex MAP objective, initial estimates are provided by a Weighted Total Least Squares (WTLS) problem posed on the appropriate product of Euclidean and manifold (Stiefel) spaces, respecting the geometry of orientation parameters. The trust-region solver initialized with SVD-based solutions ensures the optimizer is seeded near the global optimum, sharply reducing the incidence of poor local minima—an issue substantiated by the strong improvement in RMSE as compared to less accurate or random initializations.

Figure 3: Training loss and validation curves for the NDE, converging rapidly and demonstrating accurate density approximation in high dimensions.

(2) Prior Density Estimation via Neural Density Estimator (NDE).

Selection of the prior in Bayesian localization critically affects estimation quality (see Figure 4). The authors avoid the ad hoc Gaussian assumptions pervasive in the literature by learning the prior density $p(x_0)$ using a neural estimator. Simulation-based training is driven by samples generated from the linear estimator subjected to synthetic noise and process perturbations, leading to a highly flexible and empirically accurate modeling of the actual system uncertainty, as illustrated in Figure 5 and Figure 3. The NDE is validated both against analytically tractable posteriors and in ablation experiments replacing the learned prior with various Gaussian baselines (Figure 6).

Figure 4: Impact of the prior density's mean and covariance on relative localization accuracy; mismatched or broad priors significantly degrade MAP performance.

Figure 5: The NDE model architecture for mapping the true state $x$ to observed estimator outputs $\check{x}$, providing flexible and adaptive prior modeling.

Figure 6: RMSE comparison for competing prior densities; NDE and ideal Gaussian priors outperform heuristic or mismatched priors.

(3) Fixed Computational Budget via Marginalization.

The batch nature of MAP increases state dimensionality with each observation window, threatening scalability. The paper develops a fixed-lag marginalization mechanism (inspired by SLAM smoothing) to project out past state variables, maintaining constant computational cost without sacrificing estimator consistency. Hessian and Jacobian updates apply the Schur complement, standard in graphical model SLAM, for efficient windowed optimization.

Handling Real-world Sensor Modalities and Failure Modes

The methodology applies broadly to raw sensor modalities such as azimuth/elevation and UWB distance, with analytic transformations provided for mapping these to the requisite interior angles (Figure 7). Robustness mechanisms include:

• Sensor Failure Handling: Triangle geometric constraints enable continued estimation amid temporary loss or failure of angle/distance sensors, provided system rigidity is preserved (Figure 8).
• Outlier Detection: Exploits redundancy in tetrahedral rigidity—triangle inequalities and residual checks identify and filter outlier measurements (Figure 9).

Figure 7: Geometric transformation from azimuth/elevation and range to the required angle representations.

Figure 8: Mitigating impact of sensor failures by inferring missing angles/distances from local rigid geometry.

Figure 9: Outlier detectability in rigid versus minimally or redundantly constrained graphs.

Empirical Evaluation

Simulation: Localization Accuracy and Robustness

Extensive Monte Carlo simulations are conducted across varying noise scales, feasible spaces, and initializations. The localization pipeline (linear $\rightarrow$ WTLS $\rightarrow$ MAP+NDE) shows strictly monotonically improving accuracy (Figure 10). Use of low-quality initializations or poorly chosen priors yields significantly inferior results and greater variance (Figures 18, 20). Compared to standard alternatives (SDP-based estimators [jiang20193], particle/EKF/Smoothing), the proposed framework delivers consistently superior performance, especially in nontrivial noise regimes and without manual prior calibration (Figure 11).

Figure 10: RMSE of core algorithmic variants, demonstrating progressive gains from linear, WTLS, to full MAP+NDE pipelines.

Figure 12: Dependency of MAP result accuracy on initial value quality; WTLS-initialized MAP achieves the lowest RMSE.

Figure 11: Error comparison against baselines across increasing state dimensions and noise scales; the proposed method yields lower mean error and variance.

Computational Efficiency

Algorithm complexity is analyzed both theoretically and empirically (Figure 13). The real-time linear estimator has lowest cost, while the batch MAP and SDP scale polynomially with the number of time steps or robots, but remain tractable due to marginalization and initialization strategies.

Figure 13: Computational time measured for each method; 'AL' (proposed algorithm) achieves favorable scalability due to its marginalization and robust initialization.

Hardware Experiments: Indoor and Outdoor

Indoor and outdoor drone experiments validate the framework in real-world settings using angle and UWB distance, respectively. The proposed method outperforms EKF and particle filter baselines (Figures 25 and 27), with consistent RMSE gains even under significant measurement uncertainty. Notably, no other published work demonstrates 3-D relative localization in MRS using only angle and self-displacement in such deployment (Figure 14, 26).

Figure 14: Flight paths of four drones in indoor relative localization tasks.

Figure 15: Localization accuracy for all algorithms in indoor experiments using angle/self-displacement; the proposed method sets the benchmark.

(Figure 16)

Figure 16: Drone trajectories recorded during outdoor localization with range-based sensing.

(Figure 17)

Figure 17: Outdoor RMSE comparison; the proposed algorithm significantly reduces both mean and variance of position error.

Implications and Future Directions

The framework establishes that robust, scalable, and accurate 3-D relative localization is achievable in MRS using only locally available angle and self-displacement sensors, sidestepping the dependence on global heading reference, tightly controlled priors, or expensive nonlinear solvers. The modular structure allows adaptation to new sensor modalities or failure management systems. From a theoretical standpoint, the results highlight the interplay between geometric rigidity theory, statistical inference, and deep learning-based density modeling.

Potential future directions include:

• Extension to more complex robot kinematics and varying time-scale measurements (e.g., asynchronous odometry/angle updates).
• Integration with higher-level decentralized planning layers that could exploit the confidence intervals delivered by the NDE.
• Application to heterogeneous platforms (ground/air/underwater) by modular adaptation of the sensor integration block.
• Further investigation of more expressive density estimators (e.g., flow-based models) within the prior-learning pipeline.

Conclusion

This paper delivers a principled, practically validated framework for multi-robot relative localization in 3-D, combining linear geometric estimation, WTLS-initialized MAP on product manifolds, neural prior modeling, and constant-time marginalization. Strong empirical gains are demonstrated in both simulation and field experiments. The approach generalizes well to real-world noise and sensor limitations, setting a new standard for robust, accurate, and efficient collaborative localization in autonomous robot teams.

---

Reference:

For all technical details and full empirical validation, see "3-D Relative Localization for Multi-Robot Systems with Angle and Self-Displacement Measurements" (2604.01703).