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

Read PDF
Ask AI
Cite

Copy citation

DOINot available for this paper

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

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.
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.MapMethodRMSER\mathrm{RMSE}_{R}RMSEx\mathrm{RMSE}_{x}RMSERf\mathrm{RMSE}_{Rf}RMSEf\mathrm{RMSE}_{f}NEESr\mathrm{NEES}_{r}NEESp\mathrm{NEES}_{p}C95,rC_{95,r}C95,pC_{95,p}PayloadTime
GPointLocal7.26  ∣ \,\mid\, 2.572.06  ∣ \,\mid\, 0.67–  ∣ \,\mid\, –1.99  ∣ \,\mid\, 0.720.94  ∣ \,\mid\, 1.040.88  ∣ \,\mid\, 1.0797  ∣ \,\mid\, 9096  ∣ \,\mid\, 92–  ∣ \,\mid\, ––  ∣ \,\mid\, –
Pose-CI6.34  ∣ \,\mid\, 2.571.55  ∣ \,\mid\, 0.67–  ∣ \,\mid\, –1.21  ∣ \,\mid\, 0.720.73  ∣ \,\mid\, 1.040.72  ∣ \,\mid\, 1.07100  ∣ \,\mid\, 9099  ∣ \,\mid\, 92216  ∣ \,\mid\, 2163.75  ∣ \,\mid\, 8.11
Joint-CI6.22  ∣ \,\mid\, 2.231.49  ∣ \,\mid\, 0.55–  ∣ \,\mid\, –1.10  ∣ \,\mid\, 0.500.85  ∣ \,\mid\, 0.880.79  ∣ \,\mid\, 0.8596  ∣ \,\mid\, 9798  ∣ \,\mid\, 984792  ∣ \,\mid\, 47924.72  ∣ \,\mid\, 8.46
ObjectLocal6.27  ∣ \,\mid\, 2.571.91  ∣ \,\mid\, 0.6710.69  ∣ \,\mid\, 9.501.94  ∣ \,\mid\, 0.720.96  ∣ \,\mid\, 1.050.98  ∣ \,\mid\, 1.0896  ∣ \,\mid\, 8995  ∣ \,\mid\, 92–  ∣ \,\mid\, ––  ∣ \,\mid\, –
Pose-CI5.29  ∣ \,\mid\, 2.571.41  ∣ \,\mid\, 0.6710.21  ∣ \,\mid\, 9.501.19  ∣ \,\mid\, 0.720.74  ∣ \,\mid\, 1.050.78  ∣ \,\mid\, 1.08100  ∣ \,\mid\, 8999  ∣ \,\mid\, 92216  ∣ \,\mid\, 21617.65  ∣ \,\mid\, 45.59
Joint-CI5.15  ∣ \,\mid\, 2.221.34  ∣ \,\mid\, 0.559.69  ∣ \,\mid\, 8.921.07  ∣ \,\mid\, 0.500.86  ∣ \,\mid\, 0.890.86  ∣ \,\mid\, 0.8994  ∣ \,\mid\, 9598  ∣ \,\mid\, 9815220  ∣ \,\mid\, 1522018.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%95\% robot-pose coverage in the High-overlap object-map scenario.
Sc.MapMethodRMSER\mathrm{RMSE}_{R}RMSEx\mathrm{RMSE}_{x}RMSERf\mathrm{RMSE}_{Rf}RMSEf\mathrm{RMSE}_{f}NEESr\mathrm{NEES}_{r}NEESp\mathrm{NEES}_{p}C95,rC_{95,r}C95,pC_{95,p}PayloadTime
Five-RObjectLocal2.46±0.362.46\pm 0.3654.45±4.5154.45\pm 4.519.81±1.989.81\pm 1.9844.26±6.6244.26\pm 6.621.001.069493––
Pose-CI3.50±0.793.50\pm 0.7988.98±31.1688.98\pm 31.1612.02±3.0612.02\pm 3.0682.47±37.3182.47\pm 37.310.810.87999923151.12
Joint-CI{\color[rgb]{0.7,0,0}\mathbf{2.27}}\pm 0.3548.78±4.7748.78\pm 4.779.16±1.209.16\pm 1.2034.00±4.3934.00\pm 4.390.840.8598987113444.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.260.850.9098977113444.60
DIn-Local2.76±0.382.76\pm 0.3861.35±5.0061.35\pm 5.0010.14±2.0310.14\pm 2.0349.55±8.0249.55\pm 8.021.271.208690––
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.621.562.456933480351.40
TABLE II: Five-robot simulation and YCB-Video dataset-derived Monte Carlo comparison.
More in this topic

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

Explore similar work

CardsList
Mar 1, 2026

riMESA: Consensus ADMM for Real-World Collaborative SLAM

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.

Daniel McGann, Michael Kaess

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

Simultaneous Localization and MappingPose Graph OptimizationAlternating Direction Method of MultipliersDistributed Optimization
AskPDF
Jun 15, 2026

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

We introduce a data-efficient distributed Simultaneous Localization and Mapping (SLAM) framework for multi-robot operation in GPS-denied and communication-constrained environments. The framework is designed for teams of robots equipped with LiDAR, cameras, and inertial sensors, where robots may operate from independently initialized local coordinate frames and share only limited spatial overlap. Our approach constructs a scene graph by using fused RGB-LiDAR point clouds to generate both a semantically segmented point cloud layer, and a layer of discrete bounded objects, to accompany estimated robot trajectories. Scene graph matching is performed collaboratively through exchanging and matching object data with neighboring robots. To maximize communication efficiency, we utilize a multi-step data exchange and optimization process. We demonstrate the effectiveness and communication efficiency of the proposed approach using real-world datasets containing limited trajectory overlap, viewpoint variation, repeated structures, and degraded visual conditions.

Yewei Huang, Tixiao Shan, Abhinav Rajvanshi +4

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

Simultaneous Localization and MappingMulti-Robot Systems
AskPDF
Mar 11, 2026

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

This paper presents an implementation and evaluation of a Distributed Kalman--Consensus Filter (DKCF) for Multi-Object Tracking (MOT) in mobile robot networks operating under partial observability and heterogeneous localization uncertainty. A key challenge in such systems is the fusion of information from agents with differing localization quality, where frame misalignment can lead to inconsistent estimates, track duplication, and ghost tracks. To address this issue, we build upon the MOTLEE framework and retain its frame-alignment methodology, which uses consistently tracked dynamic objects as transient landmarks to improve relative pose estimates between robots. On top of this framework, we propose an uncertainty-aware adaptive consensus weighting mechanism that dynamically adjusts the influence of neighbor information based on the covariance of the transmitted estimates, thereby reducing the impact of unreliable data during distributed fusion. Local tracking is performed using a Kalman Filter (KF) with a Constant Velocity Model (CVM) and Global Nearest Neighbor (GNN) data association. simulation results demonstrate that adaptive weighting effectively protects local estimates from inconsistent data, yielding a MOTA improvement of 0.09 for agents suffering from localization drift, although system performance remains constrained by communication latency.

Niusha Khosravi, Rodrigo Ventura, Meysam Basiri

Instituto Superior Técnico University of Lisbon Lisbon, Portugal

Multi-Object TrackingMulti-Robot SystemsKalman FilteringMulti-Agent Consensus
AskPDF