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

• Published in: 2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) pp. 11064 - 11070
• Main Authors: Hao, Ning, He, Fenghua, Tian, Chungeng, Yao, Yu, Xia, Weilong
• Format: Conference Proceeding
• Language: English
• Published: IEEE 01.10.2023
• Subjects: consistency; Cooperative localization; extended Kalman filter; Filtering algorithms; Jacobian matrices; Kalman decomposition; Kalman filters; Location awareness; Monte Carlo methods; nonlinear estimation; Observability; Simultaneous localization and mapping
• ISSN: 2153-0866
• DOI: 10.1109/IROS55552.2023.10341604

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.