A Tightly Coupled Algorithm for Lidar, IMU, and GNSS with Graph Optimization

Yiqian Li, Di He, Wenxian Yu

Abstract:	With the rapid advancement of sensor technology and computational capabilities, high-precision localization has become increasingly critical in fields such as autonomous driving and robotic navigation. However, single-sensor systems exhibit significant limitations in complex environments. Multi-sensor fusion, which integrates the complementary strengths of Inertial Measurement Unit (IMU), and Light Detection and Ranging (LiDAR), has emerged as a prominent research focus. Nevertheless, existing methods still face challenges in tightly coupled integration, loop closure detection, and environmental adaptability. To address these issues, this paper proposes a tightly coupled multi-sensor localization framework based on graph optimization. The framework jointly optimizes pose estimation and other variables in a unified formulation, leveraging raw, unclassified point cloud data combined with loop closure detection to enhance localization accuracy and robustness in challenging scenarios. Experimental results on the KITTI dataset (Sequence 07) demonstrate that the proposed algorithm achieves a 64.6% improvement in maximum accuracy compared to LIOSAM and an 82.1% improvement over Fast-LIOSAM. Furthermore, the mean accuracy is improved by 42.3% and 84%, respectively. Additional tests on other datasets confirm that the proposed method yields clearer and more consistent mapping results.
Published in:	Proceedings of the 38th International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS+ 2025) September 8 - 12, 2025 Hilton Baltimore Inner Harbor Baltimore, Maryland
Pages:	567 - 575
Cite this article:	Li, Yiqian, He, Di, Yu, Wenxian, "A Tightly Coupled Algorithm for Lidar, IMU, and GNSS with Graph Optimization," Proceedings of the 38th International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS+ 2025), Baltimore, Maryland, September 2025, pp. 567-575. https://doi.org/10.33012/2025.20416
Full Paper:	ION Members/Non-Members: 1 Download Credit Sign In