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

Distributed Relative Localization Based on Ultra-WideBand and LiDAR for Multi-robot with Limited Communication
Oct 8, 2026Zhiqiang Cao, Ran Liu, Billy Pik Lik Lau +2Cooperative Localization Multi-Robot Systems
Engineering Product Development, Singapore University of Technology and Design, Singapore 487372 · School of Electrical and Electronic Engineering, Nanyang Technological University, Singapore 639798
Distributed Relative Localization for Homogeneous Multi-Robot Systems through UWB Ranging and Limited Communications
Oct 8, 2026Zhiqiang Cao, Ran Liu, Billy Pik Lik Lau +2Relative Pose Estimation Cooperative Localization
Engineering Product Development, Singapore University of Technology and Design, Singapore 487372 · School of Electrical and Electronic Engineering, Nanyang Technological University, Singapore 639798
CREPES-X: Hierarchical Bearing-Distance-Inertial Direct Cooperative Relative Pose Estimation System
Dec 31, 2025Zhehan Li, Jiadong Lu, Zheng Wang +6Relative Pose Estimation Multi-Robot Systems
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

Oct 8, 2026J/K move · Enter open · S save

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

Zhiqiang Cao, Ran Liu, Billy Pik Lik Lau, Chau Yuen+1

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

Open paper AskPDF arXiv

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.

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