Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Distributed Invariant Kalman Filter for Cooperative Localization using Matrix Lie Groups

Yizhi Zhou, Yufan Liu, Pengxiang Zhu, Xuan Wang

TL;DR

This work tackles distributed cooperative localization for a team of robots operating in 3-D environments without GPS. It develops a Distributed Invariant EKF (DInEKF) that formulates states on the matrix Lie group $SE_2(3)$, yielding state-estimate-independent Jacobians and improved estimator consistency. The algorithm proceeds with Lie-group propagation, local absolute updates, and CI-EKF-based fusion of correlated relative measurements from one-hop neighbors, ensuring consistency without a fusion center. Extensive simulations and indoor real-world experiments show that DInEKF outperforms traditional distributed EKF methods in both accuracy and consistency, highlighting the practical value of incorporating invariance and CI-based fusion in distributed multi-robot localization.

Abstract

This paper studies the problem of Cooperative Localization (CL) for multi-robot systems, where a group of mobile robots jointly localize themselves by using measurements from onboard sensors and shared information from other robots. We propose a novel distributed invariant Kalman Filter (DInEKF) based on the Lie group theory, to solve the CL problem in a 3-D environment. Unlike the standard EKF which computes the Jacobians based on the linearization at the state estimate, DInEKF defines the robots' motion model on matrix Lie groups and offers the advantage of state estimate-independent Jacobians. This significantly improves the consistency of the estimator. Moreover, the proposed algorithm is fully distributed, relying solely on each robot's ego-motion measurements and information received from its one-hop communication neighbors. The effectiveness of the proposed algorithm is validated in both Monte-Carlo simulations and real-world experiments. The results show that the proposed DInEKF outperforms the standard distributed EKF in terms of both accuracy and consistency.

Distributed Invariant Kalman Filter for Cooperative Localization using Matrix Lie Groups

TL;DR

This work tackles distributed cooperative localization for a team of robots operating in 3-D environments without GPS. It develops a Distributed Invariant EKF (DInEKF) that formulates states on the matrix Lie group , yielding state-estimate-independent Jacobians and improved estimator consistency. The algorithm proceeds with Lie-group propagation, local absolute updates, and CI-EKF-based fusion of correlated relative measurements from one-hop neighbors, ensuring consistency without a fusion center. Extensive simulations and indoor real-world experiments show that DInEKF outperforms traditional distributed EKF methods in both accuracy and consistency, highlighting the practical value of incorporating invariance and CI-based fusion in distributed multi-robot localization.

Abstract

This paper studies the problem of Cooperative Localization (CL) for multi-robot systems, where a group of mobile robots jointly localize themselves by using measurements from onboard sensors and shared information from other robots. We propose a novel distributed invariant Kalman Filter (DInEKF) based on the Lie group theory, to solve the CL problem in a 3-D environment. Unlike the standard EKF which computes the Jacobians based on the linearization at the state estimate, DInEKF defines the robots' motion model on matrix Lie groups and offers the advantage of state estimate-independent Jacobians. This significantly improves the consistency of the estimator. Moreover, the proposed algorithm is fully distributed, relying solely on each robot's ego-motion measurements and information received from its one-hop communication neighbors. The effectiveness of the proposed algorithm is validated in both Monte-Carlo simulations and real-world experiments. The results show that the proposed DInEKF outperforms the standard distributed EKF in terms of both accuracy and consistency.
Paper Structure (13 sections, 46 equations, 4 figures, 3 tables, 1 algorithm)

This paper contains 13 sections, 46 equations, 4 figures, 3 tables, 1 algorithm.

Table of Contents

  1. INTRODUCTION
  2. Preliminaries and Notations
  3. Notation and Definitions
  4. Problem Formulation
  5. Lie Group Theory
  6. Matrix Lie Group and Lie Algebra
  7. State Representation in Matrix Lie Groups
  8. Proposed Algorithm
  9. Simulation Results
  10. Real-world Experiments
  11. Conclusion
  12. Discretized IMU Propagation In Lie Group
  13. CI-EKF

Figures (4)

  • Figure 1: Cooperative Localization of a multi-robot system in an indoor environment with four Crazyflie 2.1 nano drones, based on the proposed Distributed Invariant Extended Kalman Filter (DInEKF): Each robot performs localization using only the local information (ego-motion measurements and absolute measurements), and shared information from other robots (relative measurements). The performance is evaluated using the ground truth obtained from the motion capture.
  • Figure 2: CL for four drones in 3-D environments tested with three different trajectories over 50 Monte-Carlo simulations. We plot the estimated trajectories of the first 30 trails, which shows that the estimated trajectories are close to the ground truth.
  • Figure 3: Averaged RMSE (per robot) and NEES (per robot) results in the simulated datasets.
  • Figure 4: Averaged RMSE and NEES results of dataset 1 for each robot.

Theorems & Definitions (1)

  • Remark 1: Consistency analysis of the proposed algorithm