Distributed Invariant Kalman Filter for Cooperative Localization using Matrix Lie Groups

TL;DR

This paper introduces a distributed invariant Kalman Filter based on Lie group theory for cooperative localization in multi-robot systems, improving accuracy and consistency over standard methods in 3-D environments.

Contribution

It develops a novel distributed invariant Kalman Filter on matrix Lie groups that enhances consistency and is fully distributed for multi-robot localization.

Findings

Outperforms standard distributed EKF in accuracy

Provides more consistent state estimates

Validated through simulations and real-world experiments

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.