M2C-GVIO: motion manifold constraint aided GNSS-visual-inertial odometry for ground vehicles

Abstract

Visual-Inertial Odometry (VIO) has been developed from Simultaneous Localization and Mapping (SLAM) as a low-cost and versatile sensor fusion approach and attracted increasing attention in ground vehicle positioning. However, VIOs usually have the degraded performance in challenging environments and degenerated motion scenarios. In this paper, we propose a ground vehicle-based VIO algorithm based on the Multi-State Constraint Kalman Filter (MSCKF) framework. Based on a unified motion manifold assumption, we derive the measurement model of manifold constraints, including velocity, rotation, and translation constraints. Then we present a robust filter-based algorithm dedicated to ground vehicles, whose key is the real-time manifold noise estimation and adaptive measurement update. Besides, GNSS position measurements are loosely coupled into our approach, where the transformation between GNSS and VIO frame is optimized online. Finally, we theoretically analyze the system observability matrix and observability measures. Our algorithm is tested on both the simulation test and public datasets including Brno Urban dataset and Kaist Urban dataset. We compare the performance of our algorithm with classical VIO algorithms (MSCKF, VINS-Mono, R-VIO, ORB_SLAM3) and GVIO algorithms (GNSS-MSCKF, VINS-Fusion). The results demonstrate that our algorithm is more robust than other compared algorithms, showing a competitive position accuracy and computational efficiency.