KD-EKF: A Consistent Cooperative Localization Estimator Based on Kalman Decomposition

TL;DR

This paper introduces KD-EKF, a novel cooperative localization algorithm that improves consistency by transforming the system into its observable canonical form, effectively separating observable and unobservable components.

Contribution

The paper proposes KD-EKF, a new EKF-based cooperative localization method that enhances consistency by explicitly handling system observability through Kalman decomposition.

Findings

KD-EKF outperforms existing algorithms in accuracy.

KD-EKF ensures correct observability and consistency.

Validated through simulations and real-world experiments.

Abstract

In this paper, we revisit the inconsistency problem of EKF-based cooperative localization (CL) from the perspective of system decomposition. By transforming the linearized system used by the standard EKF into its Kalman observable canonical form, the observable and unobservable components of the system are separated. Consequently, the factors causing the dimension reduction of the unobservable subspace are explicitly isolated in the state propagation and measurement Jacobians of the Kalman observable canonical form. Motivated by these insights, we propose a new CL algorithm called KD-EKF which aims to enhance consistency. The key idea behind the KD-EKF algorithm involves perform state estimation in the transformed coordinates so as to eliminate the influencing factors of observability in the Kalman observable canonical form. As a result, the KD-EKF algorithm ensures correct observability properties and consistency. We extensively verify the effectiveness of the KD-EKF algorithm through both Monte Carlo simulations and real-world experiments. The results demonstrate that the KD-EKF outperforms state-of-the-art algorithms in terms of accuracy and consistency.