InGVIO: A Consistent Invariant Filter for Fast and High-Accuracy GNSS-Visual-Inertial Odometry

Combining Global Navigation Satellite System (GNSS) with visual and inertial sensors can give smooth pose estimation without drifting in geographical coordinates. The fusion system gradually degrades to Visual-Inertial Odometry (VIO) with the number of satellites decreasing, which guarantees robust global navigation in GNSS unfriendly environments. In this letter, we propose an open-sourced invariant filter-based platform, InGVIO, to tightly fuse monocular/stereo visual-inertial measurements, along with raw data from GNSS, i.e. pseudo ranges and Doppler shifts. InGVIO gives highly competitive results in terms of accuracy and computational load compared to current graph-based and `naive' EKF-based algorithms. Thanks to our proposed key-frame marginalization strategies, the baseline for triangulation is large although only a few cloned poses are kept. Besides, landmarks are anchored to a single cloned pose to fit the nonlinear log-error form of the invariant filter while achieving decoupled propagation with IMU states. Moreover, we exploit the infinitesimal symmetries of the system, which gives equivalent results for the pattern of degenerate motions and the structure of unobservable subspaces compared to our previous work using observability analysis. We show that the properly-chosen invariant error captures such symmetries and has intrinsic consistency properties. InGVIO is tested on both open datasets and our proposed fixed-wing datasets with variable levels of difficulty. The latter, to the best of our knowledge, are the first datasets open-sourced to the community on a fixed-wing aircraft with raw GNSS.