Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Distributed Invariant Kalman Filter for Object-level Multi-robot Pose SLAM

Haoying Li, Qingcheng Zeng, Haoran Li, Yanglin Zhang, Junfeng Wu

TL;DR

A distributed invariant Kalman filter based on covariance intersection for effective multi-robot pose estimation and a combination of CI and KF to avoid overly confident or conservative estimates in multi-robot systems with intricate and unknown correlations.

Abstract

Cooperative localization and target tracking are essential for multi-robot systems to implement high-level tasks. To this end, we propose a distributed invariant Kalman filter based on covariance intersection for effective multi-robot pose estimation. The paper utilizes the object-level measurement models, which have condensed information further reducing the communication burden. Besides, by modeling states on special Lie groups, the better linearity and consistency of the invariant Kalman filter structure can be stressed. We also use a combination of CI and KF to avoid overly confident or conservative estimates in multi-robot systems with intricate and unknown correlations, and some level of robot degradation is acceptable through multi-robot collaboration. The simulation and real data experiment validate the practicability and superiority of the proposed algorithm.

Distributed Invariant Kalman Filter for Object-level Multi-robot Pose SLAM

TL;DR

A distributed invariant Kalman filter based on covariance intersection for effective multi-robot pose estimation and a combination of CI and KF to avoid overly confident or conservative estimates in multi-robot systems with intricate and unknown correlations.

Abstract

Cooperative localization and target tracking are essential for multi-robot systems to implement high-level tasks. To this end, we propose a distributed invariant Kalman filter based on covariance intersection for effective multi-robot pose estimation. The paper utilizes the object-level measurement models, which have condensed information further reducing the communication burden. Besides, by modeling states on special Lie groups, the better linearity and consistency of the invariant Kalman filter structure can be stressed. We also use a combination of CI and KF to avoid overly confident or conservative estimates in multi-robot systems with intricate and unknown correlations, and some level of robot degradation is acceptable through multi-robot collaboration. The simulation and real data experiment validate the practicability and superiority of the proposed algorithm.
Paper Structure (25 sections, 3 theorems, 48 equations, 5 figures, 2 tables, 1 algorithm)

This paper contains 25 sections, 3 theorems, 48 equations, 5 figures, 2 tables, 1 algorithm.

Table of Contents

  1. INTRODUCTION
  2. Preliminaries
  3. Covariance intersection
  4. Matrix Lie group
  5. Problem Formulation
  6. Kinematics
  7. Measurement Models
  8. Problem
  9. Methodology
  10. Local Invariant Error preintegration
  11. Example 1
  12. Example 2
  13. Invariant Kalman Filter Update
  14. New Object Initialization
  15. Local Update
  16. ...and 10 more sections

Key Result

Theorem 1

Under Assumptiona:spanning_tree, DInCIKF is stable in the sense that $\mathbf{E}[\hat{\xi}(k) \hat{\xi}(k)^{\top}]$ is bounded across time $k$ for all robots.

Figures (5)

  • Figure 1: Illustration of distributed object pose SLAM problem.
  • Figure 2: System overview.
  • Figure 3: Simulation Experiment 1: (a) Communication graph showing robot 2 with access to absolute information (points on a calibration board). The red line indicates a spanning tree rooted at the absolute source node $o$. (b) Simulation scenario with five robot trajectories and 38 object-level features. The sphere represents the 15-meter visible range of objects. Robots 1 and 5 lack object-level observations for extended periods, simulating a degenerate environment. (c) RMSE curves for average robot position and rotation estimation. The control group uses local odometry without communication, leading to inaccuracies and divergence from a prolonged lack of observation. Among the methods compared, ours achieves the highest localization accuracy.
  • Figure 4: Simulation Experiment 2: (a) Communication graph of scenario 2. (b) 3D Simulation scene in CoppeliaSim. Five robots are working collaboratively in the office. (c) Illustration of estimated robot trajectories and object pose estimation results.
  • Figure 5: YCB-V dataset Indication: (a)(c) Sample images and FoundationPose outputs of sequences 22 and 7 respectively. (b)(d) Estimation results for sequences 22 and 7.

Theorems & Definitions (6)

  • Theorem 1: Stability of DInCIKF
  • Definition 1: Observability antsaklis2007linear
  • Lemma 1
  • proof
  • Lemma 2
  • proof