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

Chenyang Liang; Liangming Chen; Baoyi Cui; Jie Mei

cs.ROApr 2, 2026

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

Authors: Chenyang Liang, Liangming Chen, Baoyi Cui, Jie Mei

Organizations: School of Intelligence Science and Engineering, Harbin Institute of Technology, Shenzhen, China. · School of Automation and Intelligent Manufacturing, and Guangdong Provincial Key Laboratory of Fully Actuated System Control Theory and Technology, Southern University of Science and Technology, China. · Guangdong Key Laboratory of Intelligent Morphing Mechanisms and Adaptive Robotics, Harbin Institute of Technology, Shenzhen, China.

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.

Figures & tables

Panel of Figure 1 — Figure 1: Impacts of prior densities on the relative localization accuracy. In this example, the truth of the initial value $\mu$ is set to zero and all prior densities are modeled as Gaussian distributions. An ideal prior density has a mean close to the truth and a small covariance. Specifically, we define $\mathcal{N}_{1}=\mathcal{N}(\mu_{0},500\cdot I_{n})$ , $\mathcal{N}_{2}=\mathcal{N}(\mu_{0.2},25\cdot I_{n})$ , $\mathcal{N}_{3}=\mathcal{N}(\mu_{0.4},25\cdot I_{n})$ , $\mathcal{N}_{4}=\mathcal{N}(\mu_{0},5\cdot I_{n})$ , $\mathcal{N}_{5}=\mathcal{N}(\mu_{0},0.2\cdot I_{n})$ , where $\mu_{z}$ satisfies $\frac{\left\|\mu_{z}-\mu\right\|}{\left\|\mu\right\|}=z$ , and $z$ denotes the offset of the mean $\mu_{z}$ with respect to $\mu$ . Figures (b)-(f) illustrate one-dimensional examples corresponding to $\mathcal{N}_{1}$ to $\mathcal{N}_{5}$ .

Figure 2: Overview of the proposed relative localization framework. Algorithm 1 formulates the relative localization problem at a single time instant as a linear equation based on angle and self-displacement measurements. Algorithm 2 enhances robustness by reformulating this linear equation as a WTLS optimization problem on manifolds. For improved accuracy over multiple time instants, a MAP estimator is employed in Algorithm 4 , where Algorithm 3 approximates the required prior probability density and the output of Algorithm 2 serves as the initial values for the MAP optimization problem.

Symbols	Meaning
$\mathchoice{\includegraphics[height=6.88889pt]{f0.png}}{\includegraphics[height=6.88889pt]{f0.png}}{\includegraphics[height=5.59721pt]{f0.png}}{\includegraphics[height=3.87495pt]{f0.png}}ijms$	Tetrahedron formed by robots $i,j,m,s$
$\triangle ijm$	Triangle formed by robots $i,j,m$
$\sum_{g}$ , $\sum_{i}$	Global coordinate frame and local coordinate frame of robot $i$
$p_{ji}^{i}\in\mathbb{R}^{3}$	Relative positions of $j$ with respect to $i$ in $\sum_{i}$
$r_{ji}\in\mathcal{S}^{1}$	Relative orientation from $\sum_{j}$ to $\sum_{i}$
$\alpha_{(\cdot)}$	Angle measurement

Table 1: Nomenclature of symbols used in this paper

Panel of Figure 3 — Figure 3: Illustration of measurements

Figure 5:

\mathchoice{\includegraphics[height=6.88889pt]{f0.png}}{\includegraphics[height=6.88889pt]{f0.png}}{\includegraphics[height=5.59721pt]{f0.png}}{\includegraphics[height=3.87495pt]{f0.png}}ijms

and geometric relation between

i,j,m,s

and

j^{\prime},m^{\prime},s^{\prime}

Figure 6: Relative positions among robots

i

j

m

and

s

with aligned coordinate frames

Panel of Figure 7 — Figure 7: Relative positions and orientations among robots $i$ , $j$ , $m$ and $s$ with unaligned coordinate frames

Figure 8: Geometric representation of a large-scale multi-robot system

Figure 9: Network structure of the NDE

Figure 10: Relationship between

x

and

\check{x}

Name	Configuration
Training data	Generated by simulation
Network structure	Feedforward neural network
Number of hidden layers	2
Number of neurons in each hidden layer	30
Active function	tanh
Prior assumption	The noise characteristics is known

Table 2: Configurations of NDE

Figure 11: The NDE takes three inputs: a model that maps $x$ to $\check{x}$ , prior density $p(x)$ and observation data $\check{x}_{o}$ . NDE executes by a) simulating samples $(\check{x}_{i},x_{i})$ from prior density $p(x)$ and the model, and b) training a neural network to fit the likelihood $p(\check{x}|x)$ based on the set of samples $\{(x_{1},\check{x}_{1}),\cdots,(x_{s},\check{x}_{s})\}$ , where $s$ denotes the number of samples, and c) evaluating the likelihood $p(\check{x}|x)$ at the observed data $\check{x}_{o}$ , and d) obtaining the estimated conditional posterior density via Bayes theorem $q(x|\check{x}_{o})\varpropto p(\check{x}_{o}|x)p(x)$ .

Figure 12: The sliding time window

Panel of Figure 13 — Figure 13: The definition and computation of angles

Figure 14: Determination of angles from inter-robot distances and heights with respect to a flat ground

Panel of Figure 15 — Figure 15: Robustness to sensor failures

Panel of Figure 16 — Figure 16: Outlier detectability among robots $i$ , $j$ , $m$ , and $s$

Panel of Figure 17 — Figure 17: Localization results of the proposed algorithms

		$\sigma=0.005$	$\sigma=0.01$	$\sigma=0.05$	$\sigma=0.08$
Algo- rithm 1	Mean	0.6888	1.1441	2.3897	2.9730
	Variance	1.4541	1.1655	4.0540	6.5170
Algo- rithm 2	Mean	0.4202	0.5492	1.4391	2.2578
	Variance	0.3592	0.8765	1.1242	0.6563
Algo- rithm 4	Mean	0.3008	0.4920	0.6893	0.8342
	Variance	0.1404	0.2980	0.3525	0.4096

Table 3: Mean and variance of RMSE shown in Figure 17 (a)

Figure 18: Localization results of the proposed algorithms with different initial values. The percentage on the

x

-axis represents the accuracy level of the initial values and the smaller the percentage, the more accurate the initial values.

	Ours	$20\%$	$30\%$	$40\%$	$50\%$
Mean	0.2070	0.2363	0.2647	0.3573	0.4185
Variance	0.0870	0.1147	0.0949	0.1486	0.2174

Table 4: Mean and variance of RMSE shown in Figure 18

Panel of Figure 19 — Figure 19: Training process and approximation accuracy of the NDE

Figure 20: Localization results of different prior densities. Specifically,

\mathcal{N}_{1}=\mathcal{N}(\mu_{0},(0.1\sigma)^{2}\cdot I)

\mathcal{N}_{2}=\mathcal{N}(\mu_{0},(100\sigma)^{2}\cdot I)

\mathcal{N}_{3}=\mathcal{N}(\mu_{0.3},(10\sigma)^{2}\cdot I)

\mathcal{N}_{4}=\mathcal{N}(\mu_{0.3},(10\sigma)^{2}\cdot I)

, where

\mu_{z}

denotes a mean offset by

z

from the true value

\mu

	Ours	$\mathcal{N}_{1}$	$\mathcal{N}_{2}$	$\mathcal{N}_{3}$	$\mathcal{N}_{4}$
Mean	0.2070	0.0176	0.2556	0.5801	0.2529
Variance	0.0870	0.0021	0.0268	0.0306	0.0108

Table 5: Mean and variance of RMSE shown in Figure 20

Panel of Figure 21 — Figure 21: Localization results comparison of different algorithms under different robot feasible spaces and noise scales

Space size	Noise scale	Mean					Variance
Space size	Noise scale	Ours	SDP	NLS	EKF	PF	Ours	SDP	NLS	EKF	PF
$5\textrm{m}\times 5\textrm{m}$ $\times 5\textrm{m}$	$\sigma$ = 0.005	0.3007	0.4678	0.7940	0.1329	0.1674	0.1404	0.6575	1.8542	0.0098	0.0029
	$\sigma$ = 0.01	0.4919	0.5920	0.8852	0.5530	0.5830	0.2980	0.5631	1.6760	0.1139	0.1538
	$\sigma$ = 0.05	0.6893	0.9869	1.3602	1.3608	1.4939	0.3525	0.6642	1.4636	0.3733	0.3411
	$\sigma$ = 0.08	0.8341	1.1999	1.4985	1.4756	1.4134	0.4096	1.2903	1.7717	0.4269	0.6573
$10\textrm{m}\times 10\textrm{m}$ $\times 10\textrm{m}$	$\sigma$ = 0.005	0.7046	1.1853	1.0296	0.4220	0.5836	0.3623	3.3074	2.5749	0.0252	0.0247

Table 6: Mean and variance of RMSE shown in Figure 21

Figure 22: Computational time of different algorithms. The term ’AL’ denotes the proposed algorithm.

	Mean	Variance
AL4	3.2168	6.5041
AL2	0.6907	0.0587
AL1	$4.6822\times 10^{-5}$	$2.0485\times 10^{-11}$
SDP	4.9373	1.2842
NLS	1.5188	0.3792
EKF	0.0001	$8.6725\times 10^{-12}$

Table 7: Mean and variance of computational time shown in Figure 22 . The term ’AL’ denotes the proposed algorithm.

Figure 23: Drone and sensors utilized in experiments

Figure 24: Traces of drones in the indoor experiment

Figure 25: Localization results comparison of different algorithms in the indoor experiment with angle measurements

	Ours	EKF	PF
Mean	1.1524	2.4690	2.2556
Variance	0.1403	0.1172	0.0801

Table 8: Mean and variance of RMSE shown in Figure 25

Figure 26: Traces of drones in the outdoor experiment

Figure 27: Localization results comparison in the outdoor experiment with distance measurements

	Ours	SDP	NLS	EKF	PF
Mean	1.3584	1.9391	3.3990	2.1595	2.7848
Variance	0.2754	0.4749	2.5693	0.5245	1.4583

Table 9: Mean and variance of RMSE shown in Figure 27

Figure 28: Measurement error in the outdoor experiment. The terms

\tilde{l}_{12}

\Delta\tilde{p}_{2,x}^{2}

denote the error in the distance measurement between drones

1

and

2

, and the error in the drone

2

’s self-displacement along the

X

-axis, respectively.

\begin{bmatrix}\begin{smallmatrix}\frac{1}{s(\alpha_{s^{\prime}j^{\prime}i}[k])}\big(\big.-s(\alpha_{s^{\prime}j^{\prime}i}[k])s(\alpha_{j^{\prime}s^{\prime}i}[k+1])R(\alpha_{s^{\prime}ij^{\prime}}[k+1])\\ +s(\alpha_{s^{\prime}j^{\prime}i}[k+1])s(\alpha_{j^{\prime}s^{\prime}i}[k])R(\alpha_{s^{\prime}ij^{\prime}}[k])\big.\big)\end{smallmatrix}&0_{2\times 1}\\ 0_{1\times 2}&\begin{smallmatrix}\frac{s(\alpha_{jsi}[k])}{s(\alpha_{jmi}[k])s(\alpha_{sji}[k])c(\alpha_{siz}[k])}\big(\big.s(\alpha_{mji}[k+1])c(\alpha_{miz}[k+1])s(\alpha_{jmi}[k])c(\alpha_{jiz}[k])\\ -s(\alpha_{mji}[k])c(\alpha_{miz}[k])s(\alpha_{jmi}[k+1])c(\alpha_{jiz}[k+1])\big.\big)\end{smallmatrix}\\ \begin{smallmatrix}\frac{1}{s(\alpha_{s^{\prime}m^{\prime}i}[k])}\big(\big.-s(\alpha_{s^{\prime}m^{\prime}i}[k])s(\alpha_{m^{\prime}s^{\prime}i}[k+1])R(\alpha_{s^{\prime}im^{\prime}}[k+1])\\ +s(\alpha_{s^{\prime}m^{\prime}i}[k+1])s(\alpha_{m^{\prime}s^{\prime}i}[k])R(\alpha_{s^{\prime}im^{\prime}}[k])\big.\big)\end{smallmatrix}&0_{2\times 1}\\ 0_{1\times 2}&\begin{smallmatrix}\frac{1}{s(\alpha_{sji}[k])c(\alpha_{siz}[k])}\big(\big.s(\alpha_{jsi}[k])s(\alpha_{sji}[k+1])c(\alpha_{jiz}[k])c(\alpha_{siz}[k+1])\\ -s(\alpha_{jsi}[k+1])s(\alpha_{sji}[k])c(\alpha_{jiz}[k+1])c(\alpha_{siz}[k])\big.\big)\end{smallmatrix}\end{bmatrix}.

(47)

Table 38

MAP Estimation

See all 8 papers

Oct 8, 2026

Distributed Relative Localization Based on Ultra-WideBand and LiDAR for Multi-robot with Limited Communication

Relative localization is crucial for a multi-robot system to collaboratively perform tasks, such as exploration and formation. However, this is highly challenging for homogeneous robots with similar appearance in GPS-denied and communication-limited environments. In this paper, we propose a fully distributed relative position estimation approach for a team of robots based on onboard UWB and LiDAR sensors, in which LiDAR is utilized to obtain the position of anonymous objects in Line-of-Sight (LOS), and UWB is used for ranging between robots. We construct two graphs, namely UWB connection graph and LiDAR connection graph, to represent the spatial relationship among objects (robots and obstacles) based on UWB and LiDAR measurements. Identification and relative position estimation are formulated as a common subgraph matching problem. A falsely-matched robot identification approach is designed to recognize the falsely-matched results caused by obstacle blockage in LiDAR field of view. These robots are then localized by leveraging the well-matched robots and the UWB ranging measurements in the UWB connection graph. We conducted experiments to evaluate the performance of our approach. The results show that the proposed approach is capable of achieving satisfactory positioning accuracy for a team of robots in a distributed manner with only exchanging limited information.

Zhiqiang Cao, Ran Liu, Billy Pik Lik Lau +2

Engineering Product Development, Singapore University of Technology and Design, Singapore 487372 · School of Electrical and Electronic Engineering, Nanyang Technological University, Singapore 639798

Cooperative Localization Multi-Robot Systems

Ask PDF

Oct 8, 2026

Distributed Relative Localization for Homogeneous Multi-Robot Systems through UWB Ranging and Limited Communications

Accurate and reliable relative localization is crucial for multi-robot applications like exploration, search, and rescue missions. LiDAR-based solutions offer high accuracy in localizing surrounding objects; however, distinguishing homogeneous robots with similar appearances remains challenging due to the lack of distinctive identification features. In this paper, we propose a fully distributed relative pose estimation approach by integrating LiDAR, UWB, and odometry measurements, allowing each robot to accurately and continuously localize its teammates without external infrastructure. Specifically, potential anonymous teammate robot clusters from LiDAR scans are tracked by a dynamic tracker. We then identify teammate robots from these tracked anonymous clusters using a joint matching strategy, ensuring reliable data association between robots and clusters. Finally, by combining the corresponding LiDAR observations, UWB ranging, and odometry measurements, each robot precisely localizes others while minimizing data exchange. The system requires only odometry data exchange through onboard UWB, eliminating the need for additional communication infrastructure like WiFi routers or mesh networks. Extensive simulation and real-world experiments demonstrate the effectiveness and reliability of the proposed relative localization approach.

Zhiqiang Cao, Ran Liu, Billy Pik Lik Lau +2

Engineering Product Development, Singapore University of Technology and Design, Singapore 487372 · School of Electrical and Electronic Engineering, Nanyang Technological University, Singapore 639798

Relative Pose Estimation Cooperative Localization Multi-Robot Systems

Ask PDF

Dec 31, 2025

CREPES-X: Hierarchical Bearing-Distance-Inertial Direct Cooperative Relative Pose Estimation System

Relative localization is essential for cooperation in autonomous multi-robot systems. Existing approaches either rely on shared environmental features or inertial assumptions, or they degrade under pairwise non-line-of-sight conditions and outliers in complex environments. Robustly and efficiently fusing inter-robot bearings, distances, and inertial measurements for tens of robots remains challenging. We present CREPES-X (Cooperative RElative Pose Estimation System with multiple eXtended features), a hierarchical relative localization framework that enhances speed, accuracy, and robustness under challenging conditions, without requiring any global information. The hardware packs infrared (IR) LEDs, an IR camera, an ultra-wideband module, and an IMU into a cube no larger than

6\,\text{cm}

on each side. On this hardware, a two-stage hierarchical estimator meets different latency, accuracy, and robustness requirements. The single-frame estimator returns instantaneous relative poses from a closed-form solution with bearing outlier rejection. The multi-frame estimator then refines these poses with IMU pre-integration under robocentric relative kinematics, using loosely- and tightly-coupled optimization. Extensive simulations and real-world experiments validate the effectiveness of CREPES-X, demonstrating robustness of up to

90\%

bearing outliers, resilience in challenging conditions, and RMSE of

7.0\,\text{cm}

and

2.2^\circ

in real-world datasets.

Zhehan Li, Jiadong Lu, Zheng Wang +6

State Key Laboratory of Industrial Control Technology, Institute of Cyber-Systems and Control, Zhejiang University, Hangzhou, China. · Huzhou Institute of Zhejiang University, Huzhou, China. · University of Michigan, Ann Arbor, MI, USA. +1

Relative Pose Estimation Multi-Robot Systems Camera Pose Estimation Cooperative Localization Robot Localization Multimodal Sensor Fusion

Ask PDF

Abstract

MAP Estimation

Explore similar work

Distributed Relative Localization Based on Ultra-WideBand and LiDAR for Multi-robot with Limited Communication

Distributed Relative Localization for Homogeneous Multi-Robot Systems through UWB Ranging and Limited Communications

CREPES-X: Hierarchical Bearing-Distance-Inertial Direct Cooperative Relative Pose Estimation System