A fast and stable GNSS-LiDAR-inertial state estimator from coarse to fine by iterated error-state Kalman filter

Simultaneous localization and mapping (SLAM) aims to solve the problems of robot localization and mapping in unknown environments. Recent related research usually uses closed-loop correction or integrate GNSS (Global Navigation Satellite System) into the optimization framework to ensure the long-term system accuracy and stability at the cost of huge computational resources. To balance efficiency and accuracy, this paper presents a fast and stable GNSS-LiDAR-inertial state estimator: GNSS, LiDAR and IMU are fused to achieve state estimation from coarse to fine, thereby improving the system accuracy and stability; the overall framework based on iterated error-state Kalman filter makes our system faster than most multi-sensor fusion SLAM. We also design a fast GNSS online initialization method and a multi-layer outlier rejection mechanism for our system. In addition, we apply backward propagation for multi-sensor motion compensation to overcome the limitations of fast motion. Finally, comprehensive experiments demonstrate that our system achieves higher accuracy and computational efficiency than the state-of-the-art navigation systems on the latest challenging public datasets, and perform equally well in the real environment.