PaperlayerView exact revision on arXiv

Source-linked AI summary

Consistent Distributed Invariant EKF SLAM with Point and Object Features

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

arXiv:2610.31221v1cs.RO

TL;DR

Unknown correlations between local estimators make distributed RI-EKF SLAM map fusion difficult, particularly because point-feature errors share the robot rotation. Joint-CI constructs a correlated pose-and-common-feature estimate in the receiving filter’s coordinates and fuses it with partial-state CI. Across simulations and a dataset-based experiment, it reduces RMSE relative to local RI-EKF and Pose-CI while maintaining consistent, mildly conservative uncertainty.

  • Problem

    In RI-EKF SLAM, point-feature errors share the robot rotation and are correlated with pose, so a neighbor’s map estimate cannot be fused directly by CI in the receiving filter’s error coordinates.

    Source: §I · p. 1

  • Method

    Joint-CI maps a neighbor’s pose and common features into the receiving filter’s error coordinates as a correlated partial state, then fuses it with the local posterior using partial-state CI.

    Source: §I · p. 1

  • Results

    Across simulations and the real-world dataset-based experiment, Joint-CI reduces RMSE relative to local RI-EKF and achieves lower RMSE than Pose-CI while maintaining consistent, mildly conservative uncertainty.

    Source: §VI · p. 7

  • Takeaways & Limitations

    The analysis establishes preservation of the global gauge directions and first-order covariance consistency of communication and fusion given consistent local posteriors.

    Source: §I · p. 2

    The complete correlated covariance payload grows quadratically with the number of common features, although it is independent of local map size.

    Source: 2) Results · p. 7

Abstract

from arXiv · show

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.

I. INTRODUCTION

Multi-robot SLAM benefits from exchanging maps, but growing optimization costs and unknown correlations complicate distributed filtering. Joint-CI addresses the RI-EKF feature-coordinate problem for point, object, and mixed maps; the formulation uses a coupled state representation and a first-order process model.

I. INTRODUCTION

  • Point-feature errors share the robot rotation, so a neighbor’s feature covariance cannot be directly combined with the receiving robot’s posterior.Ignoring pose–feature and feature–feature correlations does not guarantee consistency.Source: §I · p. 1
  • Joint-CI constructs a correlated robot–feature estimate in the receiving filter’s error coordinates, then applies partial-state CI for point, object, and mixed maps.Its analysis establishes preservation of global gauge directions and first-order covariance consistency given consistent local posteriors.Source: §I · pp. 1–2

1) State Space:

  • The coupled state group G_mp,mo represents one rotation and position vectors alongside object orientations, covering point-only and object-only maps as special cases.The state includes the robot pose and locally observed point and object features.Source: 1) State Space · p. 2

2) Process model:

  • At discrete time index n, the process model follows a first-order integration scheme commonly used in RI-EKF SLAM,; the features are static.Source: 2) Process model · p. 2

3) Observation models:

The formulation covers point-feature, object-pose, and inter-robot pose observations. It defines the distributed estimation objective and a shared RI-EKF error structure for point and object maps.

3) Observation models:

  • Robots may observe point features, object poses, and other robots’ poses.These are the three observation types specified for the system.Source: 3) Observation models · p. 2

4) Problem of Interest:

  • The objective is to build, from each neighbor, a compatible joint estimate of the receiving robot’s pose and common features, then fuse it without tracking unknown cross-correlations.The setup assumes a common arbitrary initial frame with consistent initial covariances, available cross-robot feature correspondences, and mutually independent process and observation noises.Source: 4) Problem of Interest · p. 2

A. Unified local RI-EKF formulation

  • Both point- and object-feature RI-EKF formulations use robot rotation perturbations in every position component’s error, while object features also have orientation error.This shared structure motivates the unified error representation.Source: A. Unified local RI-EKF formulation · p. 2
  • The mixed-state formulation includes point-only and object-only RI-EKF SLAM as special cases: point-only maps occur when mo = 0, while object-only maps occur when mp = 0.Source: A. Unified local RI-EKF formulation · p. 3

B. Joint communicated state in compatible coordinates

Joint communication requires the neighbor’s pose and common-feature estimate to use error coordinates compatible with the receiving filter. The mapping changes the shared rotation error and retains the covariance relationships needed for fusion.

B. Joint communicated state in compatible coordinates

  • A compatible communicated representation uses the same invariant retraction, tangent coordinates, and ordering after selection, allowing direct use in partial-state CI.The covariance construction assumes inter-robot observation noise is independent of the neighbor’s local estimation error.Source: B. Joint communicated state in… · pp. 3–4
  • The communicated partial state combines a virtual pose of the receiving robot with the neighbor’s estimates of common features.Its error uses the receiving filter’s invariant representation and corresponding component ordering.Source: B. Joint communicated state in… · p. 3
  • The mapping re-expresses common-feature tangent errors relative to the receiving robot’s virtual-pose rotation error, making the communicated covariance compatible with its local state.The covariance retains inherited robot–feature and feature–feature blocks, as well as correlations induced by inter-robot observation noise.Source: B. Joint communicated state in… · p. 4

C. Partial-state covariance intersection

The receiving robot fuses the communicated estimate with its local posterior using partial-state CI. Selection aligns the pose and common-feature components, while multiple neighbors can be handled with a multi-estimate extension.

C. Partial-state covariance intersection

  • The communicated estimate contains the receiving robot’s pose and common features, which are fused with the local posterior using standard partial-state CI.The two estimates are aligned in a common error space before fusion.Source: C. Partial-state covariance intersection · p. 4
  • A binary selection matrix extracts the local pose and common-feature errors in the same order as the communicated estimate.The corresponding local partial-state estimate uses that same ordering.Source: C. Partial-state covariance intersection · p. 4
  • For multiple neighbors, each communicated estimate is fused with the local posterior using multi-estimate partial-state CI.The nonnegative weights sum to one and are selected by minimizing the trace of the fused covariance; when components are omitted, the local-posterior weight must be positive.Source: C. Partial-state covariance intersection · p. 4

A. Gauge Preservation

A common global rotation or translation leaves the available motion and observations unchanged, defining six gauge directions. The local RI-EKF steps and subsequent fusion preserve these directions.

A. Gauge Preservation

  • States related by a fixed global-frame change are indistinguishable from the available motion and observation data.The transformation maps orientations by global rotation and positions by global rotation and translation.Source: A. Gauge Preservation · p. 4
  • The finite-horizon observability matrix annihilates the six gauge directions, so the linearized system acquires no information along them.The gauge basis encodes global rotation and translation perturbations.Source: A. Gauge Preservation · p. 4
  • Local propagation, measurement updates, and feature initialization preserve the six gauge directions for point and object features.Initialization extends the gauge-direction matrix when a new point or object feature is added.Source: A. Gauge Preservation · p. 5
  • Fusion residuals are unchanged by a common global rotation or translation, so fusion introduces no information along the six gauge directions.The same argument applies to each communicated residual in the multi-neighbor update.Source: A. Gauge Preservation · p. 5

B. Fusion consistency

The communicated estimate is first-order covariance consistent in the receiving robot’s coordinates under the stated local-posterior and noise assumptions. Partial-state CI then preserves consistency without tracking unknown cross-correlations.

B. Fusion consistency

  • A covariance is first-order consistent for an error when it upper-bounds that error’s second-moment matrix in the Loewner order.The definition is E[ee^T] ⪯ P.Source: B. Fusion consistency · p. 5
  • The communicated error covariance is first-order consistent in coordinates compatible with robot k’s local state.The derivation uses the first-order model, zero-mean inter-robot observation noise, and adopted independence assumptions.Source: B. Fusion consistency · p. 5
  • Partial-state CI preserves first-order consistency when combining the communicated estimate with Pk, without tracking unknown cross-correlation.For multiple neighbors, the extension requires nonnegative weights that sum to one.Source: B. Fusion consistency · p. 5

A. Experimental setup

The experiments cover controlled map-type comparisons, repeated information reuse, multi-neighbor communication, and a dataset-derived real-world scenario. Simulations use common environment, trajectory, range, and noise settings.

A. Experimental setup

  • The General scenario compares map types with two robots under intermittent communication, while High-overlap tests consistency under frequent communication and extensive shared-map information.The Five-robot scenario tests the construction in a more complex multi-neighbor network.Source: A. Experimental setup · p. 5
  • A dataset-derived scenario uses Sequence 7 of YCB-Video with recorded camera motion and object geometry to evaluate the backend.The scenario is illustrated in Fig. 2.Source: A. Experimental setup · p. 5
  • Each simulation has 50 features in a 100 m × 100 m environment and 2000-time-step robot trajectories, with 30 m observation and 20 m communication ranges.Mixed maps use a fixed random half of features as objects and the remainder as points.Source: A. Experimental setup · p. 5
  • High-overlap and Five-robot scenarios use mixed maps as representative of the unified formulation, while object-only maps remain for comparison with the object-specific baseline.The unified formulation reduces to point-only when mo = 0 and object-only when mp = 0.Source: A. Experimental setup · p. 5

1) Compared methods:

The evaluation compares local and distributed fusion methods across two-robot map types and communication settings, assessing accuracy, consistency, payload, and fusion time. Joint-CI achieves the lowest RMSEs for both robots in both two-robot scenarios, while its uncertainty estimates remain consistent and mildly conservative.

1) Compared methods:

  • The comparison includes Local, Pose-CI, Joint-CI, DInCIKFc-C, and DIn-Local, separating fusion effects from differences between local estimators.DInCIKFc-C uses separately constructed robot and object covariance blocks; DIn-Local supplies its no-fusion counterpart.Source: 1) Compared methods · p. 5

2) Evaluation criteria:

  • Across 100 Monte Carlo trials per scenario and map type, the evaluation measures RMSE, NEES, 95% coverage, communication payload, and fusion time.NEES is computed in each estimator’s own error coordinates; coverage near 0.95 indicates agreement with the confidence level.Source: 2) Evaluation criteria · pp. 5–6

1) Setting:

  • The General and High-overlap scenarios place robots in communication range for 10.0% and 32.0% of time steps, with 8.9 and 21.6 common features per event, respectively.The scenarios use the same environments and trajectories across point, object, and mixed-map experiments.Source: 1) Setting · p. 6, Figure 1 · p. 6

2) Results:

  • Joint-CI’s NEES stays slightly below one and coverage at or above nominal in most cases; DInCIKFc-C becomes increasingly overconfident under frequent exchanges.The paper attributes DInCIKFc-C’s consistency issue plausibly to omitted pose–object cross-covariance blocks, which need not upper-bound actual error covariance.Source: 2) Results · pp. 6–7, Figure 3 · p. 6
  • Joint-CI reduces both robots’ RMSEs and achieves the lowest RMSEs in both two-robot scenarios; Table I summarizes the paired comparisons.Pose-CI provides no effective fusion in High-overlap, while Joint-CI communicates common features with their joint covariance.Source: 2) Results · p. 6, Table I · p. 7
  • Joint-CI’s communicated covariance payload grows quadratically with common-feature count but is independent of local-map size; fusion time remains comparable to baselines.Selecting any subset of common features preserves first-order consistency, enabling payload reduction.Source: 2) Results · p. 7

C. Multi-robot network evaluation

In multi-neighbor settings, Joint-CI reduces average RMSE while maintaining consistent, mildly conservative uncertainty; its larger communication payload is the main cost. Five-robot and dataset-derived experiments evaluate the method under varied communication conditions.

1) Setting:

  • In the Five-robot scenario, at least one communication edge is active for 49.3% of time steps, with an average of 20.0 common features per edge event.The largest connected component contains at least three robots during 3.25% of time steps.Source: 1) Setting · p. 7
  • The dataset-derived experiment uses three 672-step trajectory segments with 30% overlap and reciprocal communication every ten steps over a fixed three-robot chain.Robot 2 fuses messages from both neighbors at every communication event.Source: 1) Setting · p. 7

2) Results:

  • Across the Five-robot and dataset-derived experiments, Joint-CI reduces average RMSE relative to Local while maintaining consistent, mildly conservative uncertainty.Pose-CI increases average RMSE despite conservative uncertainty; DInCIKFc-C achieves larger RMSE reductions relative to DIn-Local but has larger NEES and lower coverage.Source: 2) Results · p. 7
  • For the mixed map, Joint-CI shows the same overall behavior, while transmitting the complete joint covariance incurs a larger communication payload.The conclusion reports lower RMSE than Pose-CI and consistent, mildly conservative uncertainty under repeated information exchange.Source: 2) Results · p. 7, §VI · pp. 7–8

VI. CONCLUSION

  • Future work will study communication-aware fusion scheduling and feature selection, plus extensions to unknown initial relative poses and uncertain transformations between local maps.Source: §VI · p. 8
Loading 2610.31221v1…