Consistent Cooperative Visual-Inertial Navigation based on Matrix Lie Group

This paper introduces a novel algorithm for consistent Cooperative Visual-Inertial Navigation (CVIN) tailored for multi-robot systems using matrix Lie groups. The approach enables multiple robots to collaboratively estimate both their states and environmental features by integrating their own sensor data with shared information from neighboring robots. This shared information, which consists of commonly observed environmental features, creates geometric constraints between robots, thereby enhancing the individual robots’ state estimation accuracy. The proposed CVIN algorithm extends the Invariant Extended Kalman Filter (IEKF) from single-robot localization to a multi-robot framework, leveraging the geometric constraints between robots to further improve localization accuracy. Thanks to the inherent properties of invariant error, the algorithm naturally maintains the consistency of the multi-robot localization system, as rigorously proven through an observability analysis. Moreover, the algorithm is fully distributed, relying solely on each robot’s local measurements and information shared by one-hop communication neighbors. This structure ensures both robustness and scalability. Extensive Monte Carlo simulations demonstrate the superior performance of the proposed method in accurately estimating robot states and environmental features in 3D environments.