FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter
FAST-LIO employs tightly-coupled iEKF to fuse 1200+ features with IMU, achieving real-time robustness in 25ms per step.
Key Findings
Methodology
This paper introduces a computationally efficient LiDAR-inertial odometry framework based on a tightly-coupled iterated extended Kalman filter (iEKF). The approach fuses LiDAR feature points—extracted as planar and edge features—with IMU data, employing a novel Kalman gain formula that depends on the state dimension rather than the measurement dimension. This reduces computational load significantly, enabling real-time processing of over 1200 features per scan within 25ms. The system incorporates motion distortion compensation through forward and backward propagation, ensuring accurate pose estimation even during fast motions or noisy environments. Extensive indoor and outdoor experiments, including UAV flight tests, demonstrate high robustness, with localization errors below 0.3% and drift less than 0.08m over 32m trajectories.
Key Results
- The new Kalman gain formula reduces computation time from hundreds of milliseconds to around 1ms, enabling high-frequency updates. The system maintains localization accuracy with less than 0.3% error and 0.08m drift over 32m, outperforming LOAM+IMU in speed and stability. During UAV flights, the average per-frame processing time is 6.7ms at 50Hz, supporting real-time operation. The approach handles high feature density and dynamic scenarios effectively, even under vibration and rapid motion.
- Experimental results confirm that the proposed method achieves a significant efficiency boost without sacrificing accuracy. The system's robustness in challenging environments—such as rapid rotations exceeding 100 deg/sec—demonstrates its suitability for autonomous UAV navigation and other mobile robots.
- The key innovation lies in reformulating the Kalman gain to depend on the state dimension, drastically reducing the computational complexity associated with large measurement sets, thus enabling scalable, real-time LiDAR-inertial odometry.
Significance
This work addresses a critical bottleneck in LiDAR-inertial navigation: the high computational cost of processing dense feature measurements on resource-limited platforms like small UAVs. By introducing a novel, low-complexity Kalman gain computation, the system achieves real-time, robust localization in complex environments, paving the way for practical deployment in industrial inspection, aerial mapping, and autonomous vehicles. The method's scalability and efficiency mark a significant advancement over traditional scan-matching or sparse feature-based methods, offering a new pathway for high-density, real-time SLAM solutions.
Technical Contribution
The main technical contributions include a new formula for Kalman gain computation that depends solely on the state dimension, a tightly-coupled iEKF framework with motion distortion compensation, and an efficient feature processing pipeline. These innovations collectively reduce computational complexity, improve robustness against noise and fast motion, and enable high-density feature fusion in real-time. The approach also provides theoretical guarantees of equivalence with conventional formulas, ensuring correctness while enhancing efficiency.
Novelty
This paper is the first to derive a Kalman gain formula that depends on the state dimension rather than the measurement dimension in the context of LiDAR-inertial odometry. It combines this with a robust motion compensation mechanism and tightly-coupled filtering, setting a new standard for scalable, real-time high-density fusion. Unlike prior methods that rely on sparse features or down-sampling, this approach leverages dense features without incurring prohibitive computational costs, representing a fundamental innovation in SLAM algorithms.
Limitations
- The system relies on static initialization for gravity and bias estimation, which may limit performance in highly dynamic environments. The high feature density, while beneficial for accuracy, can introduce noise and outliers in featureless or cluttered scenes. Extreme velocities beyond tested ranges could still cause drift accumulation. Additionally, the current implementation is tailored for specific hardware platforms, and cross-platform adaptability remains to be validated.
- Further improvements are needed to handle rapid environmental changes, dynamic obstacles, and to integrate semantic information for better feature association. Computational costs, although reduced, still pose challenges for ultra-low-power embedded systems. The robustness under extreme lighting or adverse weather conditions also warrants future investigation.
Future Work
Future directions include integrating deep learning-based feature extraction to improve robustness in feature-sparse environments, extending the framework to multi-sensor fusion with cameras and other modalities, and optimizing for embedded hardware platforms. Additionally, exploring adaptive algorithms for dynamic initialization and environment-aware parameter tuning will further enhance robustness. Long-term goals involve deploying this system in large-scale autonomous operations such as urban mapping, disaster response, and autonomous delivery, where real-time, dense, and robust SLAM is essential.
AI Executive Summary
Deep Dive
Plain Language Accessible to non-experts
想象你在一个大厨房里准备一顿盛宴。每个食材代表一个特征点，而你的任务是确保每个食材都放在正确的位置。传统的方法就像用手逐个检查每个食材，既慢又容易出错。而FAST-LIO像是有一个聪明的厨师助手，它能快速扫描整个厨房，识别所有食材的位置，并根据厨房的移动自动调整每个食材的位置。即使你在快速翻炒或晃动厨房，助手也能确保所有食材都在正确的地方，不会乱跑。这样，你就能在短时间内准备出一桌美味佳肴，而不用担心出错或浪费时间。它让厨房变得更智能、更高效，也让你的烹饪变得轻松愉快。
ELI14 Explained like you're 14
想象你在玩一个超级酷的无人机游戏，你的飞机需要在城市中飞行，避开高楼和障碍物。以前的方法就像用眼睛盯着地图，一点点找路，太慢还容易迷路。而FAST-LIO就像给你的飞机装上了一个超级智能的导航助手，它可以用激光扫描周围的建筑，然后结合你飞行中的加速度和转动信息，快速算出飞机的准确位置。即使你飞得很快，助手也能帮你保持平衡，不会迷失方向。这样，你的飞机就能在复杂的环境中飞得更快、更稳，完成各种任务，比如拍照、搜救或送货。是不是很酷？
Abstract
This paper presents a computationally efficient and robust LiDAR-inertial odometry framework. We fuse LiDAR feature points with IMU data using a tightly-coupled iterated extended Kalman filter to allow robust navigation in fast-motion, noisy or cluttered environments where degeneration occurs. To lower the computation load in the presence of large number of measurements, we present a new formula to compute the Kalman gain. The new formula has computation load depending on the state dimension instead of the measurement dimension. The proposed method and its implementation are tested in various indoor and outdoor environments. In all tests, our method produces reliable navigation results in real-time: running on a quadrotor onboard computer, it fuses more than 1,200 effective feature points in a scan and completes all iterations of an iEKF step within 25 ms. Our codes are open-sourced on Github.