Implementation of Tightly-Coupled SLAM Fusion of GPS, IMU, and LiDAR for Autonomous Vehicles

Published September 18, 2026 · Category: Robotics

Overview

arXiv:2609.20321v1 Announce Type: new Abstract: Autonomous vehicles depend entirely on Simultaneous Localization and Mapping (SLAM) to navigate safely in unknown environments. However, relying on a single sensory modality introduces critical failure points: LiDAR systems degrade in featureless corridors, Inertial Measurement Units (IMUs) accumulate mathematical drift, and GPS drops frequently in urban canyons. This paper presents the implementation of a tightly-coupled SLAM fusion architecture that integrates a Velodyne 3D LiDAR, a high-frequency IMU, and GPS to achieve continuous spatial awareness. Utilizing a phased development methodology, we establish a 2D baseline to validate hardware synchronization and transform geometries before upgrading to a full 3D architecture driven by FAST-LIO2. This advanced approach uses an Iterated Error-State Kalman Filter (IESKF) to process dense 3D laser points alongside continuous inertial data, eliminating motion blur at high speeds. To eradicate long-term drift, a GTSAM pose-graph optimization back-end executes multi-modal loop closures. Evaluated across simulated environments and physical deployments, the results demonstrate that tightly-coupled 3D fusion effectively overcomes individual sensor blind spots to generate highly detailed point clouds, providing the foundational High-Definition (HD) maps required for advanced downstream autonomous planners.

Source

Originally published at arxiv.org.

Related Articles

• GAVEL: Graph World Models for Verified and Efficient Long-Horizon LLM Task Planning
• Time-Efficient Iterative Learning Planning for Safety-Critical Dynamic Obstacle Avoidance
• MoWAM: Explicit Future Motion Prediction for Efficient World Action Models