A Transformation-based Consistent Estimation Framework: Analysis, Design and Applications

In this paper, we investigate the inconsistency problem arising from observability mismatch that frequently occurs in nonlinear systems such as multi-robot cooperative localization and simultaneous localization and mapping. For a general nonlinear system, we discover and theoretically prove that the unobservable subspace of the EKF estimator system is independent of the state and belongs to the unobservable subspace of the original system. On this basis, we establish the necessary and sufficient conditions for achieving observability matching. These theoretical findings motivate us to introduce a linear time-varying transformation to achieve a transformed system possessing a state-independent unobservable subspace. We prove the existence of such transformations and propose two design methodologies for constructing them. Moreover, we propose two equivalent consistent transformation-based EKF estimators, referred to as T-EKF 1 and T-EKF 2, respectively. T-EKF 1 employs the transformed system for consistent estimation, whereas T-EKF 2 leverages the original system but ensures consistency through state and covariance corrections from transformations. To validate our proposed methods, we conduct experiments on several representative examples, including multi-robot cooperative localization, multi-source target tracking, and 3D visual-inertial odometry, demonstrating that our approach achieves state-of-the-art performance in terms of accuracy, consistency, computational efficiency, and practical realizations.

Distributed Consistent Multi-robot Cooperative Localization: A Coordinate Transformation Approach

This paper considers the problem of distributed cooperative localization (CL) via robot-to-robot measurements for a multi-robot system. We propose a distributed consistent CL algorithm. The key idea is to perform the EKF-based state estimation in a transformed coordinate system. Specifically, a coordinate transformation is constructed by decomposing the state-propagation Jacobian by which the correct observability properties are guaranteed. Moreover, the transformed state-propagation Jacobian becomes an identity matrix which is more suitable for distribution. In the proposed algorithm, a server-based framework is adopted to distributely estimate the robot pose in which each robot propagates its pose estimations and the server maintains the correlations. To reduce communication costs, only when the multi-robot system takes a robot-to-robot relative measurement, the robots and the server exchange information to update the pose estimations and the correlations. In addition, no assumptions are made about the type of robots or relative measurements. The proposed algorithm has been validated by experiments and shown to outperform the state-of-art algorithms in terms of consistency and accuracy.

KD-EKF: A Kalman Decomposition Based Extended Kalman Filter for Multi-Robot Cooperative Localization

This paper investigates the consistency problem of EKF-based cooperative localization (CL) from the perspective of Kalman decomposition, which decomposes the observable and unobservable states and allows treating them individually. The factors causing the dimension reduction of the unobservable subspace, termed error discrepancy items, are explicitly isolated and identified in the state propagation and measurement Jacobians for the first time. We prove that the error discrepancy items lead to the global orientation being erroneously observable, which in turn causes the state estimation to be inconsistent. A CL algorithm, called Kalman decomposition-based EKF (KD-EKF), is proposed to improve consistency. The key idea is to perform state estimation using the Kalman observable canonical form in the transformed coordinates. By annihilating the error discrepancy items, proper observability properties are guaranteed. More importantly, the modified state propagation and measurement Jacobians are exactly equivalent to linearizing the nonlinear CL system at current best state estimates. Consequently, the inconsistency caused by the erroneous dimension reduction of the unobservable subspace is completely eliminated. The KD-EKF CL algorithm has been extensively verified in both Monte Carlo simulations and real-world experiments and shown to achieve better performance than state-of-the-art algorithms in terms of accuracy and consistency.