Equivariant Filter Transformations for Consistent and Efficient Visual--Inertial Navigation

This paper presents an equivariant filter (EqF) transformation approach for visual--inertial navigation. By establishing analytical links between EqFs with different symmetries, the proposed approach enables systematic consistency design and efficient implementation. First, we formalize the mapping from the global system state to the local error-state and prove that it induces a nonsingular linear transformation between the error-states of any two EqFs. Second, we derive transformation laws for the associated linearized error-state systems and unobservable subspaces. These results yield a general consistency design principle: for any unobservable system, a consistent EqF with a state-independent unobservable subspace can be synthesized by transforming the local coordinate chart, thereby avoiding ad hoc symmetry analysis. Third, to mitigate the computational burden arising from the non-block-diagonal Jacobians required for consistency, we propose two efficient implementation strategies. These strategies exploit the Jacobians of a simpler EqF with block-diagonal structure to accelerate covariance operations while preserving consistency. Extensive Monte Carlo simulations and real-world experiments validate the proposed approach in terms of both accuracy and runtime.

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.