A Novel Three-Stage Robust Adaptive Filtering Algorithm for Visual-Inertial Odometry in GNSS-Denied Environments

Visual-inertial odometry (VIO) has been widely applied in the autonomous navigation and positioning of unmanned aerial vehicles (UAVs) and unmanned ground vehicles (UGVs) in global navigation satellite system (GNSS)-denied environments. However, existing VIO filtering algorithms have the defects of low positioning accuracy and weak robustness, especially in complex and changeable environments. Therefore, a novel robust adaptive VIO algorithm is proposed here. First, the H infinity criterion is introduced into the popular cubature multistate constraint Kalman filter (CMSCKF), which improves the robustness of the VIO system. Second, this article designs a characterization method to judge the uncertainty degree of VIO based on the limited memory exponential weighting theory. Finally, inspired by the idea of the Institute of Geodesy and Geophysics (IGG) III model, we further put forward a three-stage robust adaptive H infinity filtering algorithm and test the performance with the numerical simulation and the publicly available real-world Karlsruhe Institute of Technology and Toyota Technological Institute (KITTI) dataset. The experimental results demonstrate that the proposed algorithm has better filtering accuracy and robustness.