Improved FAST-LIO algorithm based on iterative non-Gaussian Kalman filter

For localization and mapping systems with 3D LIDAR and IMU, complex and diverse noises in the real environment are often encountered, and their mixed interference characteristics show non-Gaussian characteristics. They are usually assumed as Gaussian noise in existing FAST-LIO algorithms. It affects the localization and mapping accuracy and robustness. A new FAST-LIO algorithm is proposed based on iterative non-Gaussian Kalman filter to reduce the interference of mixed non-Gaussian noise. Firstly, error state iterative non-Gaussian Kalman filter is derived for localization and mapping system with 3D LIDAR and IMU disturbed by non-Gaussian noises. Then, an improved FAST-LIO algorithm is proposed on the basis of error state iterative non-Gaussian Kalman filter. In the final simulations with multiple datasets, the improved FAST-LIO algorithm is verified to successfully improve the trajectory accuracy and algorithm robustness, for the real complex environment with long trajectories.