Consistent Distributed Invariant EKF SLAM with Point and Object Features

Mengya Xu; Xianjia Yu; Ha Sier; Shoudong Huang; Tomi Westerlund

cs.ROOct 22, 2026

Consistent Distributed Invariant EKF SLAM with Point and Object Features

Authors: Mengya Xu, Xianjia Yu, Ha Sier, Shoudong Huang, Tomi Westerlund

Abstract

Multi-robot SLAM allows robots to improve their estimates by exchanging map information, but repeated exchanges create unknown correlations between local estimators. Covariance intersection (CI) can fuse estimates without tracking these correlations. In right-invariant extended Kalman filter (RI-EKF) SLAM, however, point features obtain their invariant errors through the robot rotation, so a neighbor's map estimate is expressed in its own error coordinates and is correlated with its pose, and cannot be fused by CI directly. This paper presents Joint-CI, a unified fusion method for distributed RI-EKF SLAM with point, object, and mixed point--object maps. Using a neighboring robot's local posterior and an inter-robot observation, a joint estimate of the receiving robot's pose and the common features is constructed in the receiving robot's error coordinates, retaining the robot--feature and feature--feature correlations. Partial-state CI then fuses this estimate with the receiving robot's local posterior. We show that the unified formulation and the fusion preserve the six unobservable directions associated with global rotation and translation, and that communication and fusion preserve first-order covariance consistency given consistent local posteriors. Monte Carlo simulations and a real-world dataset-derived experiment show that Joint-CI improves estimation accuracy while maintaining consistent, mildly conservative uncertainty.

Figures & tables

Panel of Figure 1 — Fig. 1: Simulation environments and ground-truth trajectories for the three evaluation scenarios. The same environment and trajectories are used for the point, object, and mixed-map experiments. For visualization, all features are shown as oriented objects with their local coordinate axes.

Panel of Figure 2 — Fig. 2: YCB-Video dataset-derived evaluation setting. Left: a representative RGB frame. Right: the recorded camera trajectory divided into three overlapping robot trajectories, together with the four object poses.

Sc.	Map	Method	$\mathrm{RMSE}_{R}$	$\mathrm{RMSE}_{x}$	$\mathrm{RMSE}_{Rf}$	$\mathrm{RMSE}_{f}$	$\mathrm{NEES}_{r}$	$\mathrm{NEES}_{p}$	$C_{95,r}$	$C_{95,p}$	Payload	Time
G	Point	Local	7.26 $\,\mid\,$ 2.57	2.06 $\,\mid\,$ 0.67	– $\,\mid\,$ –	1.99 $\,\mid\,$ 0.72	0.94 $\,\mid\,$ 1.04	0.88 $\,\mid\,$ 1.07	97 $\,\mid\,$ 90	96 $\,\mid\,$ 92	– $\,\mid\,$ –	– $\,\mid\,$ –
		Pose-CI	6.34 $\,\mid\,$ 2.57	1.55 $\,\mid\,$ 0.67	– $\,\mid\,$ –	1.21 $\,\mid\,$ 0.72	0.73 $\,\mid\,$ 1.04	0.72 $\,\mid\,$ 1.07	100 $\,\mid\,$ 90	99 $\,\mid\,$ 92	216 $\,\mid\,$ 216	3.75 $\,\mid\,$ 8.11
		Joint-CI	6.22 $\,\mid\,$ 2.23	1.49 $\,\mid\,$ 0.55	– $\,\mid\,$ –	1.10 $\,\mid\,$ 0.50	0.85 $\,\mid\,$ 0.88	0.79 $\,\mid\,$ 0.85	96 $\,\mid\,$ 97	98 $\,\mid\,$ 98	4792 $\,\mid\,$ 4792	4.72 $\,\mid\,$ 8.46
	Object	Local	6.27 $\,\mid\,$ 2.57	1.91 $\,\mid\,$ 0.67	10.69 $\,\mid\,$ 9.50	1.94 $\,\mid\,$ 0.72	0.96 $\,\mid\,$ 1.05	0.98 $\,\mid\,$ 1.08	96 $\,\mid\,$ 89	95 $\,\mid\,$ 92	– $\,\mid\,$ –	– $\,\mid\,$ –
		Pose-CI	5.29 $\,\mid\,$ 2.57	1.41 $\,\mid\,$ 0.67	10.21 $\,\mid\,$ 9.50	1.19 $\,\mid\,$ 0.72	0.74 $\,\mid\,$ 1.05	0.78 $\,\mid\,$ 1.08	100 $\,\mid\,$ 89	99 $\,\mid\,$ 92	216 $\,\mid\,$ 216	17.65 $\,\mid\,$ 45.59
		Joint-CI	5.15 $\,\mid\,$ 2.22	1.34 $\,\mid\,$ 0.55	9.69 $\,\mid\,$ 8.92	1.07 $\,\mid\,$ 0.50	0.86 $\,\mid\,$ 0.89	0.86 $\,\mid\,$ 0.89	94 $\,\mid\,$ 95	98 $\,\mid\,$ 98	15220 $\,\mid\,$ 15220	18.25 $\,\mid\,$ 41.79

TABLE I: Two-robot Monte Carlo comparison. Each paired entry is ordered as robot 1

\mid

robot 2.

Fig. 3: Robot-pose NEES and $95\%$ robot-pose coverage in the High-overlap object-map scenario.

Sc.

Map

Method

\mathrm{RMSE}_{R}

\mathrm{RMSE}_{x}

\mathrm{RMSE}_{Rf}

\mathrm{RMSE}_{f}

\mathrm{NEES}_{r}

\mathrm{NEES}_{p}

C_{95,r}

C_{95,p}

Payload

Time

Five-R

Object

Local

2.46\pm 0.36

54.45\pm 4.51

9.81\pm 1.98

44.26\pm 6.62

1.00

1.06

–

Pose-CI

3.50\pm 0.79

88.98\pm 31.16

12.02\pm 3.06

82.47\pm 37.31

0.81

0.87

231

51.12

Joint-CI

{\color[rgb]{0.7,0,0}\mathbf{2.27}}\pm 0.35

48.78\pm 4.77

9.16\pm 1.20

34.00\pm 4.39

0.84

0.85

71134

44.27

Joint-FastCI

{\color[rgb]{0.7,0,0}\mathbf{2.27}}\pm 0.34

{\color[rgb]{0.7,0,0}\mathbf{48.49}}\pm 4.84

{\color[rgb]{0.7,0,0}\mathbf{8.97}}\pm 1.15

{\color[rgb]{0.7,0,0}\mathbf{33.85}}\pm 4.26

0.85

0.90

71134

44.60

DIn-Local

2.76\pm 0.38

61.35\pm 5.00

10.14\pm 2.03

49.55\pm 8.02

1.27

1.20

–

DInCIKFc-C

{\color[rgb]{0.7,0,0}\mathbf{2.43}}\pm 0.41

{\color[rgb]{0.7,0,0}\mathbf{50.72}}\pm 7.09

{\color[rgb]{0.7,0,0}\mathbf{8.57}}\pm 1.10

{\color[rgb]{0.7,0,0}\mathbf{31.16}}\pm 2.62

1.56

2.45

4803

51.40

TABLE II: Five-robot simulation and YCB-Video dataset-derived Monte Carlo comparison.

Simultaneous Localization and Mapping

Simultaneous localization and mapping estimates a robot’s motion while building a map of an unknown environment.

See all 196 papers

riMESA: Consensus ADMM for Real-World Collaborative SLAM
Mar 1, 2026Daniel McGann, Michael KaessSimultaneous Localization and Mapping Pose Graph Optimization
The authors are with the Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, USA.
SGM-SLAM: Scene Graph Matching for Data-Efficient Distributed SLAM
Jun 15, 2026Yewei Huang, Tixiao Shan, Abhinav Rajvanshi +4Simultaneous Localization and Mapping Multi-Robot Systems
Dartmouth College, 100 N Main St, Hanover, NH 03755, USA · SRI International, 201 Washington Rd, Princeton, NJ 08540, USA · Stevens Institute of Technology, 1 Castle Point Terrace, Hoboken, NJ 07030, USA
Distributed Kalman--Consensus Filtering with Adaptive Uncertainty Weighting for Multi-Object Tracking in Mobile Robot Networks
Mar 11, 2026Niusha Khosravi, Rodrigo Ventura, Meysam BasiriMulti-Object Tracking Multi-Robot Systems
Instituto Superior Técnico University of Lisbon Lisbon, Portugal

Mar 1, 2026J/K move · Enter open · S save

riMESA: Consensus ADMM for Real-World Collaborative SLAM

Daniel McGann, Michael Kaess

The authors are with the Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, USA.

Simultaneous Localization and Mapping Pose Graph Optimization Alternating Direction Method of Multipliers Distributed Optimization

Open paper AskPDF arXiv

Collaborative Simultaneous Localization and Mapping (C-SLAM) is a fundamental capability for multi-robot teams as it enables downstream tasks like planning and navigation. However, existing C-SLAM back-end algorithms that are required to solve this problem struggle to address the practical realities of real-world deployments (i.e. communication limitations, outlier measurements, and online operation). In this paper we propose Robust Incremental Manifold Edge-based Separable ADMM (riMESA) -- a robust, incremental, and distributed C-SLAM back-end that is resilient to outliers, reliable in the face of limited communication, and can compute accurate state estimates for a multi-robot team in real-time. Through the development of riMESA, we, more broadly, make an argument for the use of Consensus Alternating Direction Method of Multipliers as a theoretical foundation for distributed optimization tasks in robotics like C-SLAM due to its flexibility, accuracy, and fast convergence. We conclude this work with an in-depth evaluation of riMESA on a variety of C-SLAM problem scenarios and communication network conditions using both synthetic and real-world C-SLAM data. These experiments demonstrate that riMESA is able to generalize across conditions, produce accurate state estimates, operate in real-time, and outperform the accuracy of prior works by a factor >7x on real-world datasets.

Abstract

Simultaneous Localization and Mapping

Explore similar work

riMESA: Consensus ADMM for Real-World Collaborative SLAM

SGM-SLAM: Scene Graph Matching for Data-Efficient Distributed SLAM

Distributed Kalman--Consensus Filtering with Adaptive Uncertainty Weighting for Multi-Object Tracking in Mobile Robot Networks