Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Covariance Intersection-based Invariant Kalman Filtering(DInCIKF) for Distributed Pose Estimation

Haoying Li, Xinghan Li, Shuaiting Huang, Chao yang, Junfeng Wu

TL;DR

This work tackles distributed pose estimation in mobile multi-agent systems where inter-agent correlations are unknown or time-varying. It introduces DInCIKF, a Covariance Intersection-based invariant Kalman filter that operates on Lie groups (notably $SE_2(3)$) to fuse relative measurements and environmental observations while maintaining consistency and avoiding overly conservative estimates. The authors provide theoretical guarantees for consistency and stability via an auxiliary upper-bound system and spanning-tree observability, and they validate the method with simulations showing improved accuracy and resilience to partial observability compared to standard EKF and InEKF baselines. The approach offers a scalable, robust framework for distributed localization in uncertain networks with practical implications for multi-robot systems and sensor networks.

Abstract

This paper presents a novel approach to distributed pose estimation in the multi-agent system based on an invariant Kalman filter with covariance intersection. Our method models uncertainties using Lie algebra and applies object-level observations within Lie groups, which have practical application value. We integrate covariance intersection to handle estimates that are correlated and use the invariant Kalman filter for merging independent data sources. This strategy allows us to effectively tackle the complex correlations of cooperative localization among agents, ensuring our estimates are neither too conservative nor overly confident. Additionally, we examine the consistency and stability of our algorithm, providing evidence of its reliability and effectiveness in managing multi-agent systems.

Covariance Intersection-based Invariant Kalman Filtering(DInCIKF) for Distributed Pose Estimation

TL;DR

This work tackles distributed pose estimation in mobile multi-agent systems where inter-agent correlations are unknown or time-varying. It introduces DInCIKF, a Covariance Intersection-based invariant Kalman filter that operates on Lie groups (notably ) to fuse relative measurements and environmental observations while maintaining consistency and avoiding overly conservative estimates. The authors provide theoretical guarantees for consistency and stability via an auxiliary upper-bound system and spanning-tree observability, and they validate the method with simulations showing improved accuracy and resilience to partial observability compared to standard EKF and InEKF baselines. The approach offers a scalable, robust framework for distributed localization in uncertain networks with practical implications for multi-robot systems and sensor networks.

Abstract

This paper presents a novel approach to distributed pose estimation in the multi-agent system based on an invariant Kalman filter with covariance intersection. Our method models uncertainties using Lie algebra and applies object-level observations within Lie groups, which have practical application value. We integrate covariance intersection to handle estimates that are correlated and use the invariant Kalman filter for merging independent data sources. This strategy allows us to effectively tackle the complex correlations of cooperative localization among agents, ensuring our estimates are neither too conservative nor overly confident. Additionally, we examine the consistency and stability of our algorithm, providing evidence of its reliability and effectiveness in managing multi-agent systems.
Paper Structure (19 sections, 9 theorems, 60 equations, 6 figures, 1 algorithm)

This paper contains 19 sections, 9 theorems, 60 equations, 6 figures, 1 algorithm.

Table of Contents

  1. INTRODUCTION
  2. Preliminaries
  3. Kalman Filter and Covariance Intersection
  4. Matrix Lie Group and Lie Algebra
  5. Problem Formulation
  6. System Kinematics
  7. Measurement Model
  8. Communication Network
  9. Distributed Invariant Kalman Filter based on Covariance Intersection
  10. Local Invariant Error Propagation
  11. Invariant Covariance Intersection to Fuse Relative Measurements
  12. Invariant Kalman Filtering Update to Fuse Environmental Measurements
  13. Theoretical Analysis
  14. Consistency Anlysis
  15. Stability Analysis
  16. ...and 4 more sections

Key Result

Lemma 1

Consider two uncorrelated, consistent and unbiased estimates $\hat{z}_1$ and $\hat{z}_2$ of a random vector $z$. The fused state estimate $\hat{z}_{\mathrm{KF}}$ with the estimated error covariance $\hat{P}_{\mathrm{KF}}$ given by KF is consistent, that is, $\mathbf{E}((z-\hat{z}_{\mathrm{KF}})(z-\h

Figures (6)

  • Figure 1: Multi-agent system with relative pose and environmental measurements.
  • Figure 2: The communication network setting for simulation.
  • Figure 3: Scenario 1. This scenario simulates a condition that involves five heterogeneous agents with varying qualities of measurement data, characterized by difference in the measurement noises and the number of observed features. This scene aims to explore whether an agent with high-quality data can assist an agent that has access only to noisy and a limited number of features.
  • Figure 4: Scenario 2. This scenario imitates an extreme environments to test the effectiveness of the proposed algorithm. It consists of three completely blind agents (agent 3, 4, 5), one agent sensing features intermittently (agent 2), and one agent observing features throughout its operational duration (agent 1). The measurement noises for agents are the same in this scenario.
  • Figure 5: RMSE evolution of scenario 1.
  • ...and 1 more figures

Theorems & Definitions (18)

  • Definition 1
  • Lemma 1: Consistency of KF
  • Lemma 2: Consistency of CI niehsen2002information
  • Remark 1
  • Theorem 1: Consistency of the DInCIKF
  • proof
  • Lemma 3
  • proof
  • Lemma 4
  • proof
  • ...and 8 more