CT-SGAL-LIO: Causal Continuous Time Refinement of Discrete LiDAR Inertial Odometry

Nam Tran, Thu Tran, Toan Nguyen, William J Beksi, Tuan DangarXiv 2026 · arXiv

TL;DR

CT-SGAL-LIO is a modular, continuous-time refinement layer that smooths the "jittery" pose outputs of standard LiDAR-Inertial Odometry (LIO) systems. By fitting a robust cubic trajectory to a sliding window of poses and querying it at a small fixed lag, it significantly reduces short-term oscillations without requiring any changes to the underlying mapping or estimation algorithms.

What Problem Does This Paper Address?

State-of-the-art LIO systems are excellent at global mapping but often produce "noisy" local trajectories. This happens because they alternate between high-rate IMU dead-reckoning and lower-rate LiDAR corrections, leading to small but noticeable jumps or oscillations in the pose stream. For a robot's controller or a planning algorithm, these high-frequency fluctuations can cause instability.

Existing solutions usually fall into two camps: modifying the filter for faster updates or building a continuous-time estimator from the ground up. The former is complex to implement, and the latter often struggles with aggressive motion or introduces significant latency. The authors of this paper propose a simpler, more modular alternative: keep the discrete LIO estimator as is and refine the pose stream after it is produced.

Methods

The proposed system, CT-SGAL-LIO, acts as a "causal refinement layer." It treats the discrete poses from an existing estimator (specifically the SGal(3)-based LIMOncello) as observations rather than ground truth.

Key technical components include:

  • Persistent State Memory: A bounded buffer that maintains a history of recent posterior poses.
  • Robust Local Trajectory Fitting: The system fits a cubic polynomial in a local $SE(3)$ Lie-algebra chart. This ensures the reconstructed orientation remains on the valid rotation manifold.
  • Fixed-Lag Query: Instead of trying to estimate the pose at the very latest timestamp (which requires unstable extrapolation), the system queries the trajectory at a small, fixed delay ($t_k - \ell$). This allows for interpolation, which is inherently more stable and accurate while remaining causal for real-time use.
  • Robust Optimization: It uses Iteratively Reweighted Least Squares (IRLS) with a Huber loss function to ignore outliers from abrupt correction events and a curvature prior to suppress oscillatory solutions.
Overview of the CT-SGAL-LIO pipeline, showing how discrete poses are buffered and refined into a continuous trajectory.

Datasets

The authors evaluated their method on 17 sequences across three diverse public benchmarks:

  • GRAND-TOUR: Data from a legged robot equipped with a Hesai XT32 LiDAR.
  • MCD (NTU Campus): Multi-campus data using a Livox Mid-70 LiDAR.
  • GEODE: Challenging environments (stairs, off-road) using a Velodyne VLP-16.
  • R-Campus: A custom 19.5-minute long-duration deployment used to assess long-horizon stability.

Key Insights and Ideas

  • Decoupling is Powerful: By treating the LIO posterior as an observation stream, the refinement module becomes "model-agnostic." It can theoretically be attached to any discrete LIO system that exposes a pose stream.
  • Interpolation > Extrapolation: The "aha moment" is that a tiny bit of causal delay (the fixed lag) allows the system to use interpolation. This provides a much smoother result than trying to predict the "now" at the very edge of the observation window.
  • Manifold-Aware Smoothing: Fitting in the Lie algebra ($se(3)$) rather than raw matrices ensures that the resulting trajectory is physically plausible and mathematically sound for 3D rotations.

Key Results

The primary metric improved is Relative Trajectory Error (RTE), which characterizes short-term motion consistency.

  • Massive Smoothing: On the GRAND-TOUR benchmark, RTE improved by up to 70%.
  • Robustness on Rough Terrain: Across seven GEODE sequences, RTE reductions ranged from 9.9% to 51.1%.
  • Long-Term Stability: In the 19.5-minute R-Campus run, the system reduced "posterior jumps" (sudden spikes in position rate) by 85.4%, while the total global drift changed by a negligible 4.7 mm.
Qualitative comparison showing how CT-SGAL-LIO (red) significantly reduces relative error compared to the baseline (blue) while maintaining the same global trajectory.
The R-Campus deployment demonstrates that the refinement remains stable over long durations, effectively suppressing high-frequency noise in the velocity estimate.

Limitations and Future Work

  • Non-Uniform Gains: The authors note that the benefits are most pronounced when the underlying discrete estimator produces noticeable short-term fluctuations. If the base estimator is already very smooth, the gains are minimal.
  • Parameter Tuning: The current implementation uses a fixed lag and fitting window. Future work could involve dynamically adapting these parameters based on the robot's current motion or the quality of the incoming pose stream.
  • Broader Validation: While tested extensively on LIMOncello, future work will involve testing the module's "plug-and-play" performance with other popular LIO frameworks.

Summary updated

Abstract

State-of-the-art LiDAR-inertial odometry methods, including those on richer state manifolds, achieve strong global accuracy, yet their pose stream alternates between propagation and correction and can oscillate over short horizons even when globally accurate. Most existing methods follow these approaches: they either modify the filter to enable faster updates or estimate a continuous-time trajectory from raw LiDAR and inertial data. The latter involves extra optimization, delaying state updates and becoming less robust during rapid rotations or accelerations. We propose CT-SGAL-LIO, a continuous-time refinement module that smooths the posterior pose stream of a discrete SGal(3) estimator without modifying the estimator itself. The key novelty is to perform continuous-time trajectory refinement directly on the projected SGal(3) posterior: we fit a cubic trajectory over a bounded window of recent poses and evaluate it at a small fixed lag. This places the query time within the observed interval, so the refined pose is obtained by interpolation rather than extrapolation, while preserving causal operation. CT-SGAL-LIO improves relative trajectory accuracy on 17 sequences from three public benchmarks. Across 17 sequences from GRAND-TOUR, MCD, and GEODE, CT-SGAL-LIO improves short-term motion accuracy while largely preserving global accuracy. RTE improves by up to 70% on GRAND-TOUR and on all seven GEODE sequences, with 9.9-51.1% reductions. A 19.5-min R-Campus run also reduces posterior jumps by 85.4% with only a 4.7 mm change in the end-to-start gap. We plan to release our source code and results to support community validation and reproducibility.
BibTeX
@misc{tran2026ct,
  author = {Nam Tran and Thu Tran and Toan Nguyen and William J Beksi and Tuan Dang},
  title = {{CT-SGAL-LIO: Causal Continuous Time Refinement of Discrete LiDAR Inertial Odometry}},
  year = {2026},
  eprint = {2611.16918},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/2611.16918},
}
Figure