PaperlayerView exact revision on arXiv

Source-linked AI summary

FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter

Wei Xu, Fu Zhang

arXiv:2010.08196v3cs.RO

TL;DR

LiDAR-inertial odometry for small UAVs must remain reliable under fast motion, noise, clutter, and large measurement sets. FAST-LIO tightly couples LiDAR features with IMU data in an iterated Kalman filter and introduces an equivalent state-dimension Kalman-gain formula. Across indoor, outdoor, and flight tests, it produces precise, real-time, reliable navigation while processing dense scans efficiently.

  • Problem

    LiDAR-inertial odometry must handle degeneration in cluttered or feature-poor environments while keeping computation manageable for scans with many measurements.

  • Method

    FAST-LIO tightly couples LiDAR feature points and IMU measurements with an iterated extended Kalman filter, using propagation for motion compensation and a state-dimension Kalman-gain formula.

  • Results

    FAST-LIO produced precise, real-time, reliable navigation in UAV, indoor, and outdoor tests, while processing 784 feature points per scan in 7.3 ms on average versus LINS at 147 points and 34.5 ms.

  • Takeaways & Limitations

    The framework provides a computationally efficient tightly coupled LiDAR-inertial odometry package for small-scale UAV platforms.

Abstract

from arXiv · show

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.

I. INTRODUCTION

FAST-LIO targets LiDAR-inertial odometry for small UAVs, addressing degeneration in cluttered environments and the computational burden of many LiDAR features. It combines tightly coupled filtering with an equivalent state-dimension Kalman-gain formulation and validates the resulting package experimentally.

  • Motivation: Solid-state LiDAR offers lightweight, low-cost, direct 3D measurements suitable for UAVs, but LiDAR SLAM still faces feature degeneration and high computational load.Degeneration is especially problematic in cluttered environments or with small fields of view, while scans may contain thousands of feature points.
  • Contribution: FAST-LIO tightly couples LiDAR feature points with IMU measurements using an iterated Kalman filter for fast motion, noise, and clutter.The method also uses formal back-propagation to compensate for motion distortion.
  • Contribution: The new Kalman-gain formula is equivalent to the conventional form while making computation depend on state dimension rather than measurement dimension.This avoids inverting a measurement-sized matrix and is intended for scans containing many feature points.
  • Evaluation: FAST-LIO is implemented as a software package designed to run on a small-scale quadrotor onboard computer.The system is evaluated in indoor and outdoor environments, including UAV flights with fast motion or intense vibration noise.
  • Related work: Loosely coupled methods reduce computation by separating scan registration and fusion, but can ignore state correlations and remain vulnerable to degenerate scan registration.The paper motivates tightly coupled fusion as a way to use raw feature points with IMU data instead.

III. METHODOLOGY

FAST-LIO processes LiDAR and IMU inputs through feature extraction and state estimation, then registers estimated poses to update a global feature map. Its manifold formulation uses ⊞ and ⊟ operators to connect states with local tangent-space representations.

  • System pipeline: FAST-LIO extracts planar and edge features from LiDAR, fuses them with IMU measurements, and estimates the state at 10Hz−50Hz.The estimated pose registers features in the global frame, updates the map, and supports subsequent point registration.
  • System pipeline: The system overview separates the overall pipeline from forward and backward propagation used during processing.The figure identifies both propagation directions as core components of FAST-LIO.
  • Notation: Table I presents important notation used throughout the method.The supplied figure materials identify the table but do not specify its individual rows or columns.
  • ⊞/⊟ operator: The ⊞ and ⊟ operators map between a manifold state and its local tangent-space representation.For a manifold of dimension n, the local representation lies in R^n; SO(3) is given as an example.

2) Continuous model:

FAST-LIO models IMU-driven motion with position, velocity, attitude, gravity, and bias states, while LiDAR scans aggregate asynchronously sampled feature points for later fusion.

  • Continuous model: The continuous kinematic model propagates IMU position, velocity, attitude, gravity, and accelerometer and gyroscope biases.Gravity is modeled as constant, while both IMU biases follow random walks.
  • Continuous model: The IMU measurements combine measured acceleration and angular velocity with bias and white-noise terms.The skew-symmetric matrix maps the cross-product operation for rotational dynamics.
  • Continuous model: The continuous model is discretized at the IMU sampling period ∆t using a zero-order holder.The resulting discrete model is indexed by individual IMU measurements and defines state, input, and noise variables.
  • Continuous model: LiDAR points are accumulated into scans because raw points arrive at rates such as 200kHz and cannot usually be processed individually.FAST-LIO sets the minimum accumulation interval to 20 ms, allowing up to 50 Hz state estimation and map updates.
  • Continuous model: Each scan contains planar and edge feature points sampled asynchronously alongside multiple IMU measurements.Feature points are sampled within the scan interval, and the final LiDAR point occurs at the scan end even when IMU samples are not aligned with scan boundaries.

C. State Estimation

FAST-LIO estimates the state in tangent-space error coordinates using iterated extended Kalman filtering, with IMU inputs driving forward state and covariance propagation to each scan time.

  • C. State Estimation: The filter represents covariance in the tangent space of the state estimate using a random error state vector.Attitude error is represented minimally with three degrees of freedom, while the remaining errors are additive.
  • C. State Estimation: Forward propagation applies the kinematic model whenever an IMU input arrives, setting process noise to zero for state propagation.The propagation interval is ∆t = τi+1 −τi.
  • C. State Estimation: The error-state dynamic model propagates covariance using state and noise transition matrices computed from the model.The white-noise covariance Q is used in the iterative covariance update.
  • C. State Estimation: Propagation continues until the end of the new scan, producing a propagated state and covariance at time tk.The covariance describes the error between the ground-truth state and the propagated state.

2) Backward Propagation and Motion Compensation:

FAST-LIO compensates LiDAR motion distortion by backward-propagating states from scan end to each feature’s sampling time, then projecting all points into the scan-end frame.

  • 2) Backward Propagation and Motion Compensation: Feature points accumulated at time tk were sampled earlier, creating a body-frame mismatch that must be corrected before fusion.The mismatch arises because each feature point has its own sampling time ρj ≤ tk.
  • 2) Backward Propagation and Motion Compensation: Backward propagation estimates the relative motion between each feature-point time ρj and the scan-end time tk.It starts from the propagated scan-end state and proceeds at feature-point frequency, using the left IMU measurement between IMU samples.
  • 2) Backward Propagation and Motion Compensation: The resulting relative pose transforms each local LiDAR measurement from its sampling frame into the scan-end LiDAR frame.The known LiDAR–IMU extrinsic transformation is included in the projection.

3) Residual computation:

FAST-LIO constructs LiDAR residuals from motion-compensated feature points and iteratively updates the propagated state with an equivalent Kalman-gain formulation whose inversions depend on state dimension.

  • 3) Residual computation: Motion-compensated feature points are transformed into the global frame using the current iterated state estimate.The transformed points are then compared with geometric features in the map.
  • 3) Residual computation: Each feature residual measures the distance from the estimated global point to its nearest mapped plane or edge.Nearby planes or edges are defined using local map feature points and their normal or orientation vectors.
  • 3) Residual computation: FAST-LIO retains residuals below a threshold such as 0.5m and treats larger residuals as outliers or newly observed points.Nearest-feature search and geometric fitting use a KD-tree built from the recent map.
  • 3) Residual computation: The measurement model is linearized around the current state estimate, with residual noise originating from LiDAR ranging and beam-directing noise.At the ground-truth state, the projected point should lie on the associated mapped plane or edge, yielding zero residual.
  • 4) Iterated state update: The iterated Kalman update repeats residual computation and state correction until the state change satisfies the convergence condition.After convergence, the resulting state estimate and covariance are taken as the optimal update.
  • 4) Iterated state update: The conventional gain requires measurement-dimension matrix inversion, whereas the equivalent new formula requires inversions in the state dimension.Because LiDAR measurement covariance is block diagonal, the revised formula supports more than 1,000 effective feature points with an 18-dimensional state.

5) The algorithm:

Algorithm 1 processes current-scan IMU inputs and LiDAR feature points through forward and backward propagation, then produces an updated state and covariance.

  • Current-scan IMU inputs and LiDAR feature points initialize the state-estimation procedure.
  • Forward propagation computes the predicted state and covariance.
  • Backward propagation compensates for scan-time motion by computing transformed feature points.
  • The Kalman gain is used to update the state estimate and covariance.
  • The algorithm outputs the current optimal state estimate and covariance.

D. Map Update

After state updating, feature points are transformed into the global frame and appended to the accumulated map.

  • Each body-frame feature point is transformed to the global frame using the updated state.
  • Transformed feature points are appended to the existing map from previous steps.

E. Initialization

The supplied passages describe initialization and report that the new Kalman-gain formula has lower complexity, but do not provide its table values or connect these details to the flight setup.

  • E. Initialization: Initialization keeps the LiDAR static for 2 seconds to estimate IMU bias and the gravity vector.
  • E. Initialization: The new Kalman-gain formula has lower complexity than the old formula under the tested system pipeline and feature-point count.
  • E. Initialization: Table II reports running times for two Kalman-gain formulas, but the supplied passage does not include the numerical entries.
  • E. Initialization: The flight experiment repeats a circular UAV path four times with periods of 6–10 s before landing at the take-off point to measure drift.

B. UAV Flight Experiments

FAST-LIO was evaluated on a quadrotor and in challenging indoor and outdoor mapping experiments, producing stable real-time odometry and favorable processing and drift results.

  • B. UAV Flight Experiments: The quadrotor carries a Livox Avia LiDAR and DJI Manifold 2 onboard computer, with odometry sent to a flight controller for circle tracking.
  • C. Indoor Experiments: 100 deg/s: indoor testing subjects the sensor suite to large rotation speeds during quickly shaken handheld motion.
  • C. Indoor Experiments: FAST-LIO outputs odometry faster and more stably than the compared LOAM implementations during large-rotation indoor experiments.
  • D. Outdoor Experiments: 0.07 m drift over 140 m: outdoor mapping with FAST-LIO yields drift smaller than 0.05%.
  • D. Outdoor Experiments: 7.3 ms versus 34.5 ms: FAST-LIO processes scans faster than LINS while using 784 versus 147 feature points per scan.
  • V. CONCLUSION: The conclusion reports precise, real-time, and reliable navigation across UAV flight, challenging indoor, and outdoor environments.

APPENDIX

The appendix rewrites the error-state model and derives the matrices F_ex and F_w using the chain rule. It also uses the matrix inverse lemma to connect an equivalent expression to the standard Kalman gain formula, alongside a comparison between LINS and FAST-LIO.

  • Experimental comparison: Figure 7 compares LINS and FAST-LIO using data collected with a Velodyne VLP-16 LiDAR and an Xsens MTiG-710 IMU.The figure identifies the green straight line at the center as the odometry output.
  • Error-state model: The error-state model is rewritten by expressing the propagated state through the boxplus and boxminus operations.The rewritten transition applies g to the estimated state, error state, and process noise before subtracting the nominal propagation.
  • Error-state model: F_ex and F_w are computed by applying the chain rule to the rewritten error-state transition.The displayed expressions evaluate the relevant partial derivatives at zero error and zero process noise.
  • Equivalent Kalman gain formula: The matrix inverse lemma is used to derive an equivalent covariance-inverse expression for the Kalman update.The derivation substitutes the lemma-based expression into the preceding formula and then recovers the standard Kalman gain formula.
Loading 2010.08196v3…