Efficient Multi-sensor Aided Inertial Navigation with Online Calibration

In this paper, we design a versatile multi-sensor aided inertial navigation system (MINS) that can efficiently fuse multi-modal measurements of IMU, camera, wheel encoder, GPS, and 3D LiDAR along with online spatiotemporal sensor calibration. Building upon our prior work [1] –[3], in this work we primarily focus on efficient LiDAR integration in a sliding-window filtering fashion. As each 3D LiDAR scan contains a large volume of 3D points which poses great challenges for real-time performance, we advocate using plane patches, which contain the environmental structural information, extracted from the sparse LiDAR point cloud to update/calibrate the system efficiently. The proposed LiDAR plane patch processing algorithm (including extraction, data association, and update) is shown to be efficient and consistent. Both Extensive Monte-Carlo simulations and real-world datasets with large-scale urban driving scenarios have been used to verify the accuracy and consistency of the proposed MINS algorithm.