M5-T9 — FAST-LIO and FAST-LIO2

Learning objectives

After this lesson, you should be able to:

• Explain forward propagation, backward propagation and the iterated update in FAST-LIO.
• Describe why the Kalman gain is reformulated for a very large number of lidar measurements.
• Explain what FAST-LIO2 changes: direct point registration and the ikd-Tree.
• Compare the filter approach with the factor-graph approach of LIO-SAM and state when each is preferable.

The problem and the intuition

If a robot knows its motion roughly (from the IMU) and has a map, then each lidar point should land on a surface. Where it does not, the robot’s pose must be off. A single point says little; thousands of points say a great deal. FAST-LIO treats every point as a tiny measurement of “distance to the nearest local plane”, which should be zero, and lets a Kalman filter turn all those near-zero-ought-to-be residuals into a correction to the state.

Two practical problems follow. The measurements arrive spread over the 100 ms of a scan, during which the robot moves: each point needs the pose at its own time (deskewing, M5-T2). And the map must be searchable and modifiable at every scan: a static k-d tree would need rebuilding. The first is solved by backward propagation with the IMU; the second by the ikd-Tree.

A filter that updates with thousands of point-to-plane residuals, with the map held in a tree that can add and delete points cheaply, gives a fast, accurate odometry without feature extraction.

The algorithm: propagate, back-propagate, iterate, update the map

Problem. Estimate the pose, velocity, IMU biases, gravity and optionally the lidar–IMU extrinsic in real time from a lidar and an IMU, with a map maintained simultaneously.

Assumptions. Local surfaces are well approximated by planes; the IMU noise is Gaussian with a slowly changing bias; the scan is short enough that IMU interpolation is accurate; the initial gravity is estimated while stationary.

1. State. The state lives on a manifold: rotation R ∈ SO(3), position p, velocity v, gyroscope and accelerometer biases b_g, b_a, gravity g, and the extrinsic (R_IL, p_IL). The filter estimates an error state δx, a vector, and folds it back into the manifold state after each update.
2. Forward propagation. For each IMU sample, integrate the nominal state and propagate the covariance P ← F P Fᵀ + G Q Gᵀ, where F is the error-state transition Jacobian (M3-T1 has the general form).
3. Backward propagation. At the end of a lidar scan, step backward through the stored IMU states and transform each point to the scan-end frame using the pose at that point’s timestamp. The result is a motion-compensated scan (M5-T2).
4. Residuals. For each deskewed point, transform it into the world with the current state estimate, find its five nearest map neighbours, fit a plane (n, d) and form r = nᵀ p_w + d. If the plane fit is poor (large residual of the fit) the point is discarded.
5. Iterated update. Compute H (the Jacobian of r with respect to the error state) and iterate: δx = K r, with the gain written in the state dimension (about 18), not the measurement dimension (thousands of points); update the state, re-linearise the residuals and repeat until δx is small. The state dimension form is K = (HᵀR⁻¹H + P⁻¹)⁻¹HᵀR⁻¹, so the large matrix is never inverted.
6. Map update. Transform the points by the corrected pose and insert them into the map. FAST-LIO2 stores them in an ikd-Tree: an incremental k-d tree that supports insertion, deletion by box and lazy re-balancing, so old distant parts of the map are removed as the robot moves and the nearest-neighbour queries stay fast.

δx

error state: small deviation of the state from the nominal, in the tangent space.

r, H, R

stacked point-to-plane residuals, their Jacobian with respect to δx and the measurement noise covariance.

P

state covariance, propagated with the IMU and reduced by updates.

ikd-Tree

incremental k-d tree: supports point insertion, box deletion and re-balancing for map maintenance.

Strengths, limitations, failure modes

A worked example: the gain in the state dimension and the cost of iteration

1. Dimensions. A scan of 4 000 downsampled points gives r of length 4 000 and H of size 4 000 × 18. A direct Kalman gain K = P Hᵀ (H P Hᵀ + R)⁻¹ requires inverting a 4 000 × 4 000 matrix: about 4 000³ / 3 ≈ 2 × 10¹⁰ floating-point operations. The information form requires an 18 × 18 inverse (about 2 000 operations) plus the product HᵀR⁻¹H, which with a diagonal R costs about 4 000 × 18² ≈ 1.3 × 10⁶ operations. A factor of about 10⁴ cheaper.
2. Equivalence. The two forms are the same estimator by the matrix inversion lemma; they differ only in which dimension is inverted.
3. Why iterate. The first residuals are computed with the predicted pose. If the prediction is 0.2 m off, the point-to-plane Jacobian (the plane normal) is evaluated at the wrong place and the first δx is biased; re-linearising at the corrected pose changes the correspondences and the normals. Typically 2–4 iterations suffice, and the loop stops when ‖δx‖ falls below 10⁻³ or so.
4. Plane fit check. With five neighbours, a plane through them has residuals; if the largest distance of a neighbour to the plane exceeds 0.1 m the points are not coplanar (an edge or noise), and the point is dropped. This is the replacement for the explicit edge/planar selection of LOAM.
5. ikd-Tree box deletion. The map is a cube of side L (say 200 m) around the robot. When the robot moves so that a face of the cube is closer than a threshold to the sensor, the slab on the far side is deleted in one operation. Without deletion the memory and query time would grow with the path length.

Compare with LIO-SAM (M5-T8): the same sensors, but a graph of keyframes with a registration as one factor; here there is no registration step at all, the points enter the filter directly.

Engineering implications and limitations

Engineering implications

• Check the downsample resolution. 0.3–0.5 m indoors and 0.5–1 m outdoors are typical; too fine and the filter wastes time, too coarse and thin structures vanish.
• Set the covariances honestly. The lidar noise R and the IMU noise Q set the balance. A very small R makes the filter trust a poor map.
• Log the iteration count and the effective number of matched points. A drop in matched points is the earliest sign of a degenerate scene.
• Add loop closure separately. FAST-LIO has none; extensions (for example FAST-LIO-SLAM, using Scan Context) add a pose graph on top.
• Mind the map size. The ikd-Tree is cheap but not free; set the local map length for the environment.
• Choose filter or graph deliberately. Filters are faster per update and good for odometry; graphs make loop closure and re-linearisation natural. Many production systems combine them.

Limits

The filter linearises once per update (with a few iterations) and then forgets the linearisation points. An early error in a bad scene stays in the state. Keeping consistent covariances in the presence of the manifold rotation (error-state formalism) is delicate; and the point-to-plane model is a local approximation that fails on vegetation, glass and moving objects. These are the reasons that the research community also builds on smoothing and on learned or probabilistic maps (Module 8).

Common misconceptions

Engineering takeaways

1. FAST-LIO is an iterated error-state Kalman filter: IMU prediction, point-to-plane updates from the lidar, state on a manifold.
2. Backward propagation deskews each point with the IMU pose at its own time.
3. The gain in the state dimension replaces a measurement-sized inverse with an 18 × 18 one.
4. FAST-LIO2 matches raw points to an ikd-Tree map and removes feature extraction.
5. A filter bounds cost but cannot revisit the past: add loop closure and a consistency check for long runs.

References and further reading

Primary papers and authoritative documentation.

• Xu, W. and Zhang, F. (2021). FAST-LIO: A fast, robust LiDAR-inertial odometry package by tightly-coupled iterated Kalman filter. IEEE Robotics and Automation Letters 6(2).
• Xu, W., Cai, Y., He, D., Lin, J. and Zhang, F. (2022). FAST-LIO2: Fast direct LiDAR-inertial odometry. IEEE Transactions on Robotics 38(4).
• Cai, Y., Xu, W. and Zhang, F. (2021). ikd-Tree: An incremental k-d tree for robotic applications. arXiv:2102.10808.
• Bloesch, M. et al. (2015). Robust visual inertial odometry using a direct EKF-based approach. IEEE/RSJ IROS: the error-state, iterated-update lineage.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Both LIO-SAM and FAST-LIO produce an odometry whose drift grows with distance. The final lesson of the module shows how the map is managed and how a place seen before is recognised, to close loops.

Related content

Nothing below is required to finish this lesson.

• Before
  M5-T8: LIO-SAM

  Describe tightly coupled LiDAR-inertial smoothing with factor graphs and iSAM2.

• Lab
  M5-L4: Diagnose LiDAR SLAM Drift

  Drift accumulates in a long run: attribute it to registration, IMU bias or missing loop closure and fix it.