MEKAWIRE
ExpertarXiv

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

Nam Tran, Thu Tran, Toan Nguyen, William J Beksi, Tuan Dang
From the 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.

From the abstract. Our summary is in progress.

Full paper

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

Nam Tran, Thu Tran, Toan Nguyen, William J Beksi, Tuan Dang

Published under CC BY 4.0. Reproduced with attribution; original at arXiv:2611.16918 (PDF).

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 \mathrm{SGal}(3) estimator without modifying the estimator itself. The key novelty is to perform continuous-time trajectory refinement directly on the projected \mathrm{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.

I Introduction

LiDAR–inertial odometry (LIO) underlies mapping, planning, and control on mobile robots, and downstream consumers routinely query the pose stream at rates decoupled from the LiDAR scan rate. State-of-the-art tightly coupled iterated filters, most recently formulated on the Galilean manifold \mathrm{SGal}(3) [1], achieve strong global trajectory precision. However, the state stream that such filters publish alternates between IMU-propagated states and LiDAR-corrected states, which can exhibit different local error characteristics. As a result, a globally accurate trajectory can still contain short-horizon oscillations, visible even on our own baseline: on the GRAND-TOUR [2] HAUS_1 sequence, the discrete estimator we build on already achieves a low global error, yet its relative motion error over short intervals still fluctuates, and CT-SGAL-LIO reduces this short-term error by 67% without altering the global trajectory (Fig. 1).

Fig. 1: CT-SGAL-LIO maintains a persistent fixed-lag memory of incoming LIO posterior states and reconstructs a continuous trajectory by robust metric-weighted cubic fitting in a local SE(3) tangent chart. At the delayed query time t_{k}^{\star}=t_{k}-\ell, the fitted trajectory provides the refined pose and its linear and angular velocities. The orange segment marks the illustrated local support.

Existing remedies for this local inconsistency modify the discrete estimator itself. One direction raises the state-update rate by restructuring the filter to process individual points rather than whole scans. A second direction replaces the discrete state altogether with a continuous-time trajectory representation fitted jointly to raw LiDAR and, where available, inertial residuals. This approach works well, but it has two drawbacks, discussed in Section II. Smooth parametric trajectories can struggle with aggressive motion, and fitting them with future measurements introduces delay. Neither direction considers a simpler alternative: keep the discrete LIO estimator and its map untouched, and refine only the pose stream after it is produced.

We answer this question with CT-SGAL-LIO, an online continuous-time refinement layer that treats the discrete posterior as an observation rather than as a state to be re-derived. CT-SGAL-LIO maintains a persistent, bounded-memory buffer of recent posterior poses and fits a robust cubic trajectory over this window in a local \mathrm{SE}(3) Lie-algebra chart. The refined trajectory is evaluated at a fixed lag, placing the query time inside the observed window rather than at its boundary. This lets us obtain the pose by interpolation rather than extrapolation, without sacrificing causality. Because CT-SGAL-LIO consumes only the timestamped \mathrm{SE}(3) projection of the estimator’s posterior state and feeds nothing back into the filter or its map, it is decoupled from the underlying estimator and can in principle attach to any discrete LiDAR-inertial system that exposes a pose stream. In this paper, we instantiate it on top of LIMOncello [1], the estimator whose \mathrm{SGal}(3) posterior it refines.

Our contributions are as follows.

  • We propose a model-agnostic, causal, fixed-lag continuous-time refinement layer that operates purely on the posterior pose stream of a discrete LIO estimator, without coupling to raw sensor residuals or to the map.
  • We formulate this refinement as robust cubic trajectory fitting in a local \mathrm{SE}(3) Lie-algebra chart, combining iteratively reweighted least squares with a curvature prior tailored to short, bounded temporal windows.
  • We show that attaching CT-SGAL-LIO to LIMOncello’s \mathrm{SGal}(3) posterior consistently improves short-term motion accuracy while generally preserving global trajectory accuracy across 17 sequences from three public benchmarks (GRAND-TOUR, MCD [3], and GEODE [4]). Specifically, CT-SGAL-LIO reduces RTE by up to 70% on GRAND-TOUR and by 9.9–51.1% on all seven GEODE sequences, while ranking first or second in RTE on all six evaluated MCD sequences.

The remainder of this paper is organized as follows. Section II reviews discrete-time LIO, manifold state representations, continuous-time trajectory estimation, and fixed-lag smoothing. Section III overviews the system architecture. Section IV details the fixed-lag continuous-time refinement formulation. Section V presents our experimental evaluation. Section VI concludes the paper.

II Related Work

II-A Discrete-time LIO on Manifold State Representations

Most LiDAR odometry systems follow LOAM [5]: extract geometric features, estimate scan-to-pose correspondences, and register to a map. Tightly coupled LiDAR–inertial designs then became standard, either as sliding-window smoothers [6] or as iterated filters standardized by IKFoM [7]. FAST-LIO [8] introduced IMU back-propagation with a Kalman gain that scales with the state dimension rather than the measurement dimension, and FAST-LIO2 [9] removed hand-engineered feature extraction by registering raw points directly to an incremental k-d tree, ikd-Tree [10]. This family publishes only one pose per scan, and Point-LIO [11, 12] shows that this scan-rate update imposes a Nyquist-limited bandwidth. The state representation from this work is typically a bundle manifold SO(3)\times\mathbb{R}^{6}, which ignores the coupling between rotation and translation; richer Lie groups restore this coupling along the nested hierarchy SO(3)<SE(3)<SE_{2}(3)<SGal(3) [13], with SE_{2}(3) underpinning invariant filtering [14] and SGal(3) additionally carrying a temporal coordinate. LIMOncello [1] is, to our knowledge, the first tightly coupled LIO system on SGal(3) within IKFoM, and is the estimator we build on: because SE(3) is a proper subgroup of SGal(3), the pose \Pi_{SE(3)}(G_{i}) is a well-defined projection, and it is the quantity on which our refinement layer operates. We leave this discrete-time estimator unchanged and instead regularize the state sequence that it already produces.

II-B Continuous-time trajectory estimation

Continuous-time methods represent the trajectory as a function of time, so measurements can be fused at their true timestamps and motion distortion is estimated jointly with the motion [15]. Two parameterizations are commonly used. Piecewise-linear models assume constant velocity over short intervals, as in standard deskewing. Cubic B-splines use more parameters but give a smoother trajectory. CT-ICP [16] uses a begin and an end pose per scan and deliberately allows inter-scan discontinuity, while Traj-LO [17] regularises short linear segments. Cubic B-splines are used in sliding-window LiDAR–inertial fusion by CLIC and SLICT2 [18, 19] and, more recently, inside a recursive filter, RESPLE [20], which embeds spline control points directly in an iterated Kalman filter and avoids sliding-window optimization altogether. DLIO [21] instead sidesteps control-point fitting entirely, integrating a closed-form constant-jerk model. Smooth trajectory methods like CT-ICP, DLIO, and Point-LIO argue that control-point fitting can smooth out rapid motion, while RESPLE finds cubic splines sufficiently flexible in practice. Continuous-time methods also introduce latency because future measurements must be collected before the trajectory parameters are well constrained and the state can be queried [11]. CT-ICP, CLIC, SLICT2, and RESPLE optimize their trajectory parameters directly against raw LiDAR (and, where used, inertial) residuals, so the continuous-time representation is their estimator rather than a separable layer on top of it. CT-SGAL-LIO instead operates only on the timestamped pose stream that LIMOncello already publishes, with no feedback into the filter or its map, which lets the refinement layer attach to any discrete LiDAR-inertial estimator that exposes a pose stream.

II-C Smoothing and Fixed-Lag Estimation in LiDAR Odometry

Fixed-lag smoothing bounds the estimation delay by withholding the output until a short buffer of subsequent measurements becomes available [22]. In LiDAR odometry, smoothing is usually applied instead as a factor-graph back-end [6] or a sliding window of spline control points with marginalisation [18, 19, 17]. A related formulation is Wildcat [23], which fits a cubic B-spline to sample poses by IRLS, following the offline systems of Bosse and Zlot [24]; there, however, the spline is internal to the estimator, since the sample poses are themselves optimized against IMU and surfel residuals and the interpolated trajectory is fed back to update correspondences. Because Wildcat fits the spline inside its pipeline, the spline is part of the estimator rather than a separate refinement stage. Comparing our method directly with Wildcat would therefore confound the effect of trajectory refinement with differences in the underlying estimator. To isolate the effect of our refinement, we compare against LIMOncello and RESPLE, the discrete estimators whose posterior pose streams serve as input to our method. CT-SGAL-LIO treats these posterior poses as observations, fits a continuous trajectory over a bounded history window, and evaluates the trajectory at a fixed lag so that the query time lies inside the observed interval. This keeps the refinement causal while leaving the original LiDAR–inertial estimator and map unchanged.

Fig. 2: Overview of CT-SGAL-LIO. Posterior states from the discrete SGal(3) LIO estimator are maintained in a bounded temporal memory and reconstructed by covariance-aware robust cubic fitting. The resulting continuous trajectory is evaluated at the delayed query time t_{k}^{\star}=t_{k}-\ell to obtain the refined state.

III System Overview

The overall architecture of CT-SGAL-LIO is illustrated in Fig. 2. A discrete SGal(3)-based LiDAR–inertial estimator provides high-rate posterior states after IMU propagation and LiDAR correction. Instead of treating each posterior state independently, CT-SGAL-LIO keeps a short history of recent states and fits a continuous trajectory through them; CT-SGAL-LIO uses the recent posterior poses to fit a cubic trajectory that maps each pose to a local tangent space, and its covariance determines how strongly it influences the fit. A temporal regularizer limits unnecessary curvature. The trajectory is evaluated at the interior delayed time t_{k}^{\star}=t_{k}-\ell and mapped back to the pose manifold to obtain the refined pose and motion state. This design keeps the continuous-time refinement module separate from the LiDAR front end while remaining causal and suitable for online use.

IV Methodology

IV-A Problem Formulation

We consider a LiDAR–inertial estimator whose posterior state at time t_{i} evolves on the special Galilean group,

\mathbf{G}_{i}=\begin{bmatrix}\mathbf{R}_{i}&\mathbf{v}_{i}&\mathbf{p}_{i}\\
\mathbf{0}^{\top}&1&t_{i}\\
\mathbf{0}^{\top}&0&1\end{bmatrix}\in\mathrm{SGal}(3),
(1)

where \mathbf{R}_{i}\in\mathrm{SO}(3), \mathbf{p}_{i}\in\mathbb{R}^{3}, and \mathbf{v}_{i}\in\mathbb{R}^{3} are the estimated orientation, position, and velocity. We use LIMOncello [1] as the discrete underlying estimator. CT-SGAL constructs a continuous-time readout from its pose posterior \mathbf{T}_{i}=(\mathbf{R}_{i},\mathbf{p}_{i})\in\mathrm{SE}(3) and the associated covariance \boldsymbol{\Sigma}_{i}.

The estimator produces timestamped posterior states \{(t_{i},\mathbf{T}_{i},\boldsymbol{\Sigma}_{i})\}_{i=1}^{k}. Because IMU propagation and LiDAR correction occur at different rates, the sampled trajectory can contain short-term variations around correction events. These variations become visible when the trajectory is queried at a higher rate than the LiDAR updates.

CT-SGAL-LIO processes this stream online using a time-bounded pose buffer. At the arrival of state k, the new posterior (t_{k},\mathbf{T}_{k},\boldsymbol{\Sigma}_{k}) is inserted into the buffer \mathcal{M}_{k}, while states older than the memory horizon \Delta_{m} are removed by \mathcal{P}_{\Delta_{m}}. The output is placed at the fixed-lag time t_{k}^{\star}=t_{k}-\ell, and the active window is defined as

\mathcal{W}_{k}=\left\{(t_{i},\mathbf{T}_{i},\boldsymbol{\Sigma}_{i})\in\mathcal{M}_{k}\;\middle|\;t_{k}^{\star}-W\leq t_{i}\leq t_{k}\right\}.
(2)

Here, W is the history retained before the query time, and \ell is the output lag. Unlike pairwise interpolation, each CT-SGAL estimate uses all posterior states in \mathcal{W}_{k}, while \mathcal{M}_{k} persists across successive updates.

The active interval [t_{k}^{\star}-W,t_{k}] is normalized before fitting. Its center is c_{k}=(t_{k}^{\star}-W+t_{k})/2, with half-width h=(W+\ell)/2. Each sample is represented by the normalized coordinate

u_{i}=\frac{t_{i}-c_{k}}{h},\qquad u_{k}^{\star}=\frac{t_{k}^{\star}-c_{k}}{h}=\frac{W-\ell}{W+\ell}.
(3)

This maps [t_{k}^{\star}-W,t_{k}] to [-1,1]. For W>0 and \ell>0, the query lies strictly inside the observed interval. The fit therefore uses states both before and after t_{k}^{\star} without accessing measurements later than the current processing time t_{k}.

Directly fitting rotation matrices does not preserve \mathrm{SO}(3). The first pose in \mathcal{W}_{k} serves as the local reference, \mathbf{T}_{r,k}=(\mathbf{R}_{r,k},\mathbf{p}_{r,k}). Each pose is represented by its translation and rotation relative to this reference:

\mathbf{z}_{i}=\begin{bmatrix}\mathbf{p}_{i}-\mathbf{p}_{r,k}\\
\operatorname{Log}\!\left(\mathbf{R}_{r,k}^{\top}\mathbf{R}_{i}\right)\end{bmatrix}\in\mathbb{R}^{6}.
(4)

The resulting six-dimensional coordinates are modeled by a cubic polynomial. Using \mathbf{b}(u)=[1,u,u^{2},u^{3}]^{\top} and \mathbf{A}_{k}=[\mathbf{A}_{p,k}^{\top}\ \mathbf{A}_{R,k}^{\top}]^{\top}, with \mathbf{A}_{p,k},\mathbf{A}_{R,k}\in\mathbb{R}^{3\times 4}, the fitted trajectory is

\hat{\mathbf{z}}_{k}(u)=\mathbf{A}_{k}\mathbf{b}(u).
(5)

The corresponding continuous pose is

\hat{\mathbf{T}}_{k}(u)=\begin{bmatrix}\mathbf{R}_{r,k}\operatorname{Exp}\!\left(\mathbf{A}_{R,k}\mathbf{b}(u)\right)&\mathbf{p}_{r,k}+\mathbf{A}_{p,k}\mathbf{b}(u)\\
\mathbf{0}^{\top}&1\end{bmatrix}.
(6)

The exponential map ensures that the reconstructed orientation remains in \mathrm{SO}(3) for every u.

Let \mathcal{I}_{k} contain the indices of the observations in \mathcal{W}_{k}, and define \mathbf{r}_{i}(\mathbf{A})=\mathbf{z}_{i}-\mathbf{A}\mathbf{b}(u_{i}). We estimate the polynomial coefficients by

\displaystyle\mathbf{A}_{k}^{\star}=\arg\min_{\mathbf{A}} \\ \displaystyle\sum_{i\in\mathcal{I}_{k}}\rho_{\delta}\!\left(\|\mathbf{r}_{i}(\mathbf{A})\|_{\boldsymbol{\Omega}_{i}}\right) \\ \displaystyle+\sum_{q\in\mathcal{Q}}\left\|\mathbf{A}\ddot{\mathbf{b}}(q)\right\|_{\boldsymbol{\Lambda}}^{2}+\lambda_{d}\|\mathbf{A}\|_{F}^{2},

where \|\mathbf{x}\|_{\boldsymbol{\Omega}}=\sqrt{\mathbf{x}^{\top}\boldsymbol{\Omega}\mathbf{x}}, \rho_{\delta} is the Huber loss [25], and \mathcal{Q}=\{-1,-0.5,0,0.5,1\} contains the curvature evaluation points. We use \boldsymbol{\Lambda}=\operatorname{diag}(\lambda_{p}\mathbf{I}_{3},\lambda_{R}\mathbf{I}_{3}) to tune translational and rotational curvature separately.

The first term fits the recent posterior states using their uncertainty. The Huber loss limits the effect of abrupt correction events, while the curvature term suppresses oscillatory solutions. Thus, each update combines multiple propagation and correction states rather than refining the newest pose independently.

IV-B Online Estimation and Implementation Details

CT-SGAL maintains \mathcal{M}_{k} as a time-ordered buffer. A state with the same timestamp as the newest entry replaces that entry, while out-of-order states are discarded. Fitting starts once the buffer spans the required interval and is repeated whenever the query time t_{k}^{\star} advances.

The input covariance controls the weight of each posterior pose. We define a minimum covariance \boldsymbol{\Sigma}_{0}=\operatorname{diag}(\sigma_{p}^{2}\mathbf{I}_{3},\sigma_{R}^{2}\mathbf{I}_{3}). Before inversion, a valid covariance is conditioned as

\displaystyle\tilde{\boldsymbol{\Sigma}}_{i} \\ \displaystyle=\operatorname{EigFloor}\!\left(\operatorname{sym}(\boldsymbol{\Sigma}_{i})+\boldsymbol{\Sigma}_{0},\epsilon\right), \\ \displaystyle\boldsymbol{\Omega}_{i} \\ \displaystyle=\tilde{\boldsymbol{\Sigma}}_{i}^{-1}.

where \operatorname{sym}(\boldsymbol{\Sigma})=(\boldsymbol{\Sigma}+\boldsymbol{\Sigma}^{\top})/2, and \operatorname{EigFloor}(\cdot,\epsilon) replaces eigenvalues smaller than \epsilon by \epsilon. If the input covariance is unavailable or invalid, we use \tilde{\boldsymbol{\Sigma}}_{i}=\boldsymbol{\Sigma}_{0}. Thus, posterior poses with lower uncertainty receive greater weight in the fit.

The robust objective in Eq. (7) is solved using iteratively reweighted least squares (IRLS). At iteration j, the residual of observation i is \mathbf{r}_{i}^{(j)}=\mathbf{r}_{i}(\mathbf{A}_{k}^{(j)}). Its covariance-normalized magnitude is

e_{i}^{(j)}=\sqrt{\mathbf{r}_{i}^{(j)\top}\boldsymbol{\Omega}_{i}\mathbf{r}_{i}^{(j)}}.
(9)

The corresponding Huber weight is

w_{i}^{(j)}=\begin{cases}1,&e_{i}^{(j)}\leq\delta,\\[2.84526pt]
\delta/e_{i}^{(j)},&e_{i}^{(j)}>\delta.\end{cases}
(10)

Thus, \boldsymbol{\Omega}_{i} accounts for the uncertainty of each posterior state, while w_{i}^{(j)} reduces the influence of residuals that exceed the Huber threshold.

With the robust weights fixed, the cubic coefficients form a 24-dimensional weighted least-squares problem. We vectorize \mathbf{A}_{k} as \mathbf{a}_{k}=\operatorname{vec}(\mathbf{A}_{k}^{\top})\in\mathbb{R}^{24} and solve the normal system

\mathbf{H}_{k}^{(j)}\mathbf{a}_{k}^{(j+1)}=\mathbf{g}_{k}^{(j)}.
(11)

Here, \mathbf{H}_{k}^{(j)} and \mathbf{g}_{k}^{(j)} collect the weighted data term, the curvature term, and the damping term in Eq. (7). Since the cubic trajectory has four coefficients for each of the six pose coordinates, the system size remains 24\times 24 regardless of the total trajectory length.

After the final IRLS iteration, the fitted trajectory is evaluated at the fixed-lag coordinate u_{k}^{\star}. The refined pose is

\hat{\mathbf{T}}_{k}^{\star}=\hat{\mathbf{T}}_{k}(u_{k}^{\star}).
(12)

Because u=(t-c_{k})/h, we have du/dt=1/h. The translational velocity therefore follows directly from the derivative of the cubic basis:

\hat{\mathbf{v}}_{k}^{\star}=\mathbf{A}_{p,k}^{\star}\dot{\mathbf{b}}(u_{k}^{\star}),\qquad\dot{\mathbf{b}}(u)=\frac{1}{h}[0,1,2u,3u^{2}]^{\top}.
(13)

For rotation, we use a centered Lie-group difference around the query time. With \Delta u=\Delta t/h, the angular velocity is

\hat{\boldsymbol{\omega}}_{k}^{\star}=\frac{1}{2\Delta t}\hat{\mathbf{R}}_{k}^{\star}\operatorname{Log}\!\left(\hat{\mathbf{R}}_{k}(u_{k}^{\star}-\Delta u)^{\top}\hat{\mathbf{R}}_{k}(u_{k}^{\star}+\Delta u)\right).
(14)

The online output is (\hat{\mathbf{T}}_{k}^{\star},\hat{\mathbf{v}}_{k}^{\star},\hat{\boldsymbol{\omega}}_{k}^{\star}), with all quantities obtained from the same fitted trajectory.

V Evaluations

In this section, we present an extensive experimental evaluation of CT-SGAL-LIO, focusing on both global trajectory accuracy and short-term motion consistency. The evaluation is conducted on GRAND-TOUR, MCD, and GEODE, which collectively provide diverse sensing configurations, motion patterns, and challenging geometric conditions. Our experiments investigate whether the proposed fixed-lag continuous-time refinement can reduce local trajectory oscillations while preserving the global accuracy of the underlying discrete-time estimator. In addition to the benchmark evaluation, we use a long-duration R-Campus deployment to assess the long-horizon stability of CT-SGAL and its effect on the global structure of the underlying LIO trajectory.

V-A Experimental Setup

We evaluated CT-SGAL-LIO on a total of 17 sequences from three public datasets: four GRAND-TOUR sequences, six sequences from the NTU Campus subset of MCD, and seven challenging sequences from GEODE. The selected datasets also span different LiDAR–inertial sensing configurations: the GRAND-TOUR sequences use a Hesai XT32 LiDAR with a NovAtel CPT7 inertial system; for MCD NTU Campus we use the Livox Mid-70 LiDAR with a VectorNav VN100 IMU; and for GEODE we use the \alpha platform, a Velodyne VLP-16 LiDAR with an Xsens MTi-30 IMU. Together, these datasets cover a wide range of sensing configurations, motion patterns, and environmental conditions, including indoor and outdoor scenes, day and night operation, unstructured terrain, and geometrically challenging scenarios.

Fig. 3: Qualitative comparison between LIMOncello and CT-SGAL-LIO on the day01 sequence of the MCD NTU Campus dataset. (a) Estimated trajectories compared with the ground truth, (b) ATE over time, and (c) RTE evaluated at different time intervals \Delta t. CT-SGAL-LIO preserves a trajectory close to the baseline while substantially reducing short-term relative motion error.
Fig. 4: Long-duration online behavior of CT-SGAL on the R-Campus deployment. (a) The refined trajectory remains closely aligned with the LIO posterior over a 19.5-min sequence, while the inset shows a locally continuous CT trajectory reconstructed from discrete posterior samples. (b) CT-SGAL strongly attenuates consecutive-state posterior jumps, reducing p_{95}(r_{k}) from 16.5 to 2.4 m/s (85.4\%), while introducing only a 4.7 mm change in the 3-D end-to-start gap.

We compare CT-SGAL-LIO with its discrete-time baseline LIMOncello, together with RESPLE and FAST-LIO2. Trajectory accuracy is evaluated using the absolute trajectory error (ATE) RMSE and the relative trajectory error (RTE) RMSE at a temporal interval of 0.1\,\mathrm{s}, following standard trajectory evaluation practice [26] and computed with the evo toolbox [27]. Estimated trajectories are synchronized with the corresponding ground truth, and ATE is computed after a global rigid \mathrm{SE}(3) alignment. ATE measures global trajectory accuracy, whereas RTE characterizes short-term relative motion consistency and is therefore particularly relevant for evaluating the proposed continuous-time refinement. Lower values indicate better performance for both metrics.

V-B Qualitative Results

Figure 3 illustrates the effect of CT-SGAL-LIO on the MCD day01 sequence. While the refined and baseline trajectories remain nearly identical at the global scale, CT-SGAL-LIO consistently reduces the relative motion error over short evaluation intervals. This indicates that the proposed fixed-lag reconstruction primarily removes high-frequency trajectory fluctuations without noticeably altering the underlying global estimate.

TABLE I: Quantitative comparison on the GRAND-TOUR dataset in terms of ATE RMSE and RTE RMSE at a time interval of 0.1 s. All values are reported in meters. Lower is better, and the best result for each metric is highlighted in bold.
  • ATE
    ARC1
    RTE0.1s
    ATE
    Con_1
    RTE0.1s
    ATE
    Con_3
    RTE0.1s
    ATE
    HAUS_1
    RTE0.1s
    No data
  • LIMOncello
    ARC1
    0.0686
    0.0296
    Con_1
    0.0678
    0.0310
    Con_3
    0.0236
    0.0158
    HAUS_1
    0.0277
    0.0257
  • RESPLE
    ARC1
    0.0251
    0.0305
    Con_1
    0.0630
    0.0343
    Con_3
    0.0635
    0.0236
    HAUS_1
    0.0393
    0.0307
  • FAST-LIO2
    ARC1
    0.0250
    0.0284
    Con_1
    0.0311
    0.0321
    Con_3
    15.6995
    0.0843
    HAUS_1
    0.0214
    0.0285
  • CT-SGAL-LIO
    ARC1
    0.0654
    0.0123
    Con_1
    0.0635
    0.0090
    Con_3
    0.0208
    0.0085
    HAUS_1
    0.0189
    0.0084
TABLE II: Quantitative comparison on the NTU Campus subset of the MCD dataset. We evaluate day and night sequences recorded using a Livox Mid70 LiDAR and a VN100 IMU. ATE RMSE and RTE RMSE at a time interval of 0.1 s are reported in meters. Lower is better. The best and second-best results for each metric are highlighted in bold and underlined, respectively.
  • ATE
    day01
    RTE0.1s
    ATE
    day02
    RTE0.1s
    ATE
    day10
    RTE0.1s
    ATE
    night04
    RTE0.1s
    ATE
    night08
    RTE0.1s
    ATE
    night13
    RTE0.1s
    No data
  • LIMOncello
    day01
    0.7416
    0.0810
    day02
    0.2246
    0.0185
    day10
    3.1404
    0.0312
    night04
    0.8877
    0.0221
    night08
    1.0046
    0.0232
    night13
    1.6388
    0.0690
  • RESPLE
    day01
    0.4693
    0.0276
    day02
    0.2634
    0.1712
    day10
    1.6488
    0.1335
    night04
    –
    –
    night08
    8.6461
    0.1082
    night13
    1.1414
    0.0442
  • FAST-LIO2
    day01
    0.8075
    0.0213
    day02
    0.3176
    0.1161
    day10
    0.9861
    0.1404
    night04
    0.9041
    0.0936
    night08
    1.4239
    0.1219
    night13
    2.8225
    0.0595
  • CT-SGAL-LIO
    day01
    0.7389
    0.0244
    day02
    0.2245
    0.0168
    day10
    3.1403
    0.0327
    night04
    0.8878
    0.0208
    night08
    1.0045
    0.0235
    night13
    1.6376
    0.0501
TABLE III: Quantitative comparison on GEODE. ATE RMSE and RTE RMSE at \Delta t=0.1 s are reported in meters. All methods are evaluated on the same ground-truth associations within each sequence. Lower is better; best and second-best results are shown in bold and underlined, respectively.
  • Method
    Waterways Long
    ATE
    RTE0.1
    Waterways Med.
    ATE
    RTE0.1
    Waterways Short
    ATE
    RTE0.1
    Offroad2
    ATE
    RTE0.1
    Offroad4
    ATE
    RTE0.1
    Offroad7
    ATE
    RTE0.1
    Stairs
    ATE
    RTE0.1
  • LIMOncello
    Waterways Long
    44.8979
    0.0619
    Waterways Med.
    14.3201
    0.0682
    Waterways Short
    34.8550
    0.0635
    Offroad2
    0.4449
    0.0592
    Offroad4
    0.2814
    0.0587
    Offroad7
    0.3636
    0.0576
    Stairs
    3.8750
    0.0481
  • RESPLE
    Waterways Long
    105.6537
    0.1278
    Waterways Med.
    137.5077
    0.1596
    Waterways Short
    1.9073
    0.0812
    Offroad2
    0.4627
    0.0643
    Offroad4
    0.2968
    0.0654
    Offroad7
    0.3803
    0.0615
    Stairs
    4.4882
    0.0952
  • FAST-LIO2
    Waterways Long
    140.7757
    0.1040
    Waterways Med.
    65.9198
    0.0901
    Waterways Short
    30.9266
    0.0484
    Offroad2
    0.4580
    0.0518
    Offroad4
    0.2883
    0.0539
    Offroad7
    0.3708
    0.0506
    Stairs
    1.3639
    0.0292
  • CT-SGAL-LIO
    Waterways Long
    44.8954
    0.0420
    Waterways Med.
    14.3195
    0.0387
    Waterways Short
    34.8558
    0.0515
    Offroad2
    0.4398
    0.0289
    Offroad4
    0.2798
    0.0310
    Offroad7
    0.3608
    0.0337
    Stairs
    3.8693
    0.0433

Finally, Fig. 4 examines the long-horizon behavior of the proposed refinement. Over a 19.5-min R-Campus deployment containing approximately 120.8k posterior states, CT-SGAL remains tightly anchored to the underlying LIO trajectory, with only a 4.7 mm change in the 3-D end-to-start gap. At the local scale, however, the reconstructed trajectory is considerably more temporally consistent: the 95th percentile of the consecutive-state position-rate statistic r_{k} decreases from 16.5 to 2.4 m/s, corresponding to an 85.4\% reduction. This result shows that CT-SGAL can continuously suppress posterior jumps without introducing noticeable long-term trajectory deformation.

V-C Quantitative Results

Tables I–III report the ATE and RTE results across GRAND-TOUR, MCD, and GEODE. Overall, CT-SGAL-LIO shows its clearest advantage in short-term motion consistency. On GRAND-TOUR, it achieves the lowest RTE on all four evaluated sequences, while also obtaining the best ATE on Con_3 and HAUS_1. In particular, the RTE reductions over LIMOncello are substantial across all four sequences, confirming that the continuous-time refinement consistently suppresses local posterior fluctuations.

A similar trend is observed on MCD. CT-SGAL-LIO ranks first or second in RTE on all six day and night sequences, while its ATE remains close to that of the underlying LIMOncello estimator. The greatest improvements occur on day01 and night13, whereas on day10 and night08 the two methods perform almost identically. This suggests that the refinement remains stable across different operating conditions and does not trade global accuracy for local smoothness.

On GEODE, CT-SGAL-LIO reduces RTE at \Delta t=0.1 s relative to LIMOncello on all seven sequences, with reductions ranging from 9.9% on Stairs to 51.1% on Offroad2. It achieves the lowest RTE on five sequences and the second-lowest on the remaining two. ATE remains close to the input posterior and is lowest on all three Offroad sequences and on Waterways Long and Medium. These results match the intended behavior of CT-SGAL: the largest change occurs in short-horizon relative motion, while the globally aligned trajectory remains anchored to the discrete estimator.

Across the three benchmarks, the results show that CT-SGAL-LIO does not systematically alter the global trajectory estimate; instead, it improves local motion consistency while generally preserving the ATE of the discrete-time estimator.

V-D Ablation Study

TABLE IV: GEODE ablation on nominal data. Values are geometric-mean ratios to the input posterior over seven sequences (input =1; lower is better). The p99 residual is computed from a 50-ms local linear fit at 100 Hz.
  • Input posterior
    ATE
    1.0000
    RTE0.1
    1.0000
    p99 residual
    1.0000
  • No output lag (\ell=0)
    ATE
    0.9964
    RTE0.1
    0.6918
    p99 residual
    0.0173
  • No curvature term
    ATE
    0.9961
    RTE0.1
    0.6394
    p99 residual
    0.0081
  • No Huber loss
    ATE
    0.9962
    RTE0.1
    0.6314
    p99 residual
    0.0035
  • Full CT-SGAL
    ATE
    0.9963
    RTE0.1
    0.6370
    p99 residual
    0.0035

We ablate the output lag in Eq. (2), the curvature term in Eq. (7), and the Huber loss in Eq. (7) on the seven GEODE sequences. The full model uses (W,\ell)=(0.3,0.2) s. For the no-output-lag variant, we set (W,\ell)=(0.5,0) s such that the same 0.5-s history is retained. The other two variants remove the curvature term and replace the Huber loss with ordinary least squares, respectively.

As shown in Table IV, the full model reduces RTE0.1 by 36.3% with negligible change in ATE. The fixed output lag yields a further 7.9% reduction in RTE over the zero-lag variant, while the curvature term reduces the p99 residual by approximately half. Removing the Huber loss changes the nominal RTE by less than 1%, indicating that isolated posterior outliers are uncommon in these sequences.

We further evaluate its robustness by injecting identical 0.2-m translation impulses into 0.5\% of the posterior states on three Offroad sequences. Under these perturbations, the Huber loss reduces the geometric-mean p99 residual from 0.721 to 0.482 mm, while ATE changes from 0.35365 to 0.35373 m and RTE from 0.03048 to 0.03110 m. These results indicate that the Huber loss primarily suppresses isolated posterior errors rather than affecting nominal trajectory accuracy.

VI Conclusions

We presented CT-SGAL-LIO, a continuous-time refinement layer for the posterior pose stream of a discrete LiDAR–inertial estimator with causality. The method leaves the estimator and its map unchanged, using a history of posterior poses to fit a robust cubic trajectory in a local SE(3) and evaluating it at a fixed lag so that the query lies within the observed interval. Across 17 sequences from GRAND-TOUR, MCD, and GEODE, CT-SGAL-LIO mainly improves short-term motion consistency while preserving the global trajectory. It reduces RTE on all GRAND-TOUR sequences, with gains of up to 70%, while on GEODE it achieves the best results on 10 of 14 metrics and second-best on 3, including the best ATE and RTE across all three Offroad sequences. A 19.5-min R-Campus run also shows an 85.4% reduction in posterior jumps with only a 4.7 mm change in the end-to-start gap. The gains are not uniform, however, suggesting that refinement is most useful when the discrete posterior contains noticeable short-term fluctuations. Future work will test the approach with other LIO estimators and adapt the lag and fitting window to the incoming posterior’s behavior.

References

  1. [1] C. Pérez-Ruiz and J. Solà, “Limoncello: Iterated error-state kalman filter on the sgal (3) manifold for fast lidar-inertial odometry,” arXiv preprint arXiv:2512.19567, 2025.
  2. [2] J. Frey, T. Tuna, F. Fu, K. Patterson, T. Xu, M. Fallon, C. Cadena, and M. Hutter, “Grandtour: A legged robotics dataset in the wild for multi-modal perception and state estimation,” arXiv e-prints, pp. arXiv–2602, 2026.
  3. [3] T.-M. Nguyen, S. Yuan, T. H. Nguyen, P. Yin, H. Cao, L. Xie, M. Wozniak, P. Jensfelt, M. Thiel, J. Ziegenbein, et al., “Mcd: Diverse large-scale multi-campus dataset for robot perception,” in 2024 IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR). IEEE, 2024, pp. 22 304–22 313.
  4. [4] Z. Chen, Y. Qi, D. Feng, X. Zhuang, H. Chen, X. Hu, J. Wu, K. Peng, and P. Lu, “Heterogeneous lidar dataset for benchmarking robust localization in diverse degenerate scenarios,” The International Journal of Robotics Research, vol. 45, no. 1, pp. 6–22, 2026.
  5. [5] J. Zhang, S. Singh, et al., “Loam: Lidar odometry and mapping in real-time.” in Robotics: Science and systems, vol. 2, no. 9. Berkeley, CA, 2014, pp. 1–9.
  6. [6] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, “Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping,” in 2020 IEEE/RSJ international conference on intelligent robots and systems (IROS). IEEE, 2020, pp. 5135–5142.
  7. [7] D. He, W. Xu, and F. Zhang, “Symbolic representation and toolkit development of iterated error-state extended kalman filters on manifolds,” IEEE Transactions on Industrial Electronics, vol. 70, no. 12, pp. 12 533–12 544, 2023.
  8. [8] W. Xu and F. Zhang, “Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3317–3324, 2021.
  9. [9] W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, “Fast-lio2: Fast direct lidar-inertial odometry,” IEEE Transactions on Robotics, vol. 38, no. 4, pp. 2053–2073, 2022.
  10. [10] Y. Cai, W. Xu, and F. Zhang, “ikd-tree: An incremental kd tree for robotic applications,” arXiv preprint arXiv:2102.10808, 2021.
  11. [11] D. He, W. Xu, N. Chen, F. Kong, C. Yuan, and F. Zhang, “Point-lio: robust high-bandwidth light detection and ranging inertial odometry,” Advanced Intelligent Systems, vol. 5, no. 7, p. 2200459, 2023.
  12. [12] N. Tran, T. Tran, H. Phan, T. Luu, T. Nguyen, W. J. Beksi, and T. Dang, “Hp2-slam: Adaptive hybrid icp for robust and efficient lidar slam,” arXiv preprint arXiv:2608.14996, 2026.
  13. [13] J. Kelly, “All about the galilean group sgal (3),” arXiv preprint arXiv:2312.07555, 2023.
  14. [14] A. Barrau and S. Bonnabel, “The invariant extended kalman filter as a stable observer,” IEEE Transactions on Automatic Control, vol. 62, no. 4, pp. 1797–1812, 2016.
  15. [15] W. Talbot, J. Nubert, T. Tuna, C. Cadena, F. Dümbgen, J. Tordesillas, T. D. Barfoot, and M. Hutter, “Continuous-time state estimation methods in robotics: A survey,” IEEE Transactions on Robotics, 2025.
  16. [16] P. Dellenbach, J.-E. Deschaud, B. Jacquet, and F. Goulette, “Ct-icp: Real-time elastic lidar odometry with loop closure,” in 2022 international conference on robotics and automation (ICRA). IEEE, 2022, pp. 5580–5586.
  17. [17] X. Zheng and J. Zhu, “Traj-lo: In defense of lidar-only odometry using an effective continuous-time trajectory,” IEEE Robotics and Automation Letters, vol. 9, no. 2, pp. 1961–1968, 2024.
  18. [18] J. Lv, X. Lang, J. Xu, M. Wang, Y. Liu, and X. Zuo, “Continuous-time fixed-lag smoothing for lidar-inertial-camera slam,” IEEE/ASME Transactions on Mechatronics, vol. 28, no. 4, pp. 2259–2270, 2023.
  19. [19] T.-M. Nguyen, X. Xu, T. Jin, Y. Yang, J. Li, S. Yuan, and L. Xie, “Eigen is all you need: Efficient lidar-inertial continuous-time odometry with internal association,” IEEE Robotics and Automation Letters, vol. 9, no. 6, pp. 5330–5337, 2024.
  20. [20] Z. Cao, W. Talbot, and K. Li, “Resple: Recursive spline estimation for lidar-based odometry,” IEEE Robotics and Automation Letters, 2025.
  21. [21] K. Chen, R. Nemiroff, and B. T. Lopez, “Direct lidar-inertial odometry: Lightweight lio with continuous-time motion correction,” arXiv preprint arXiv:2203.03749, 2022.
  22. [22] T.-C. Dong-Si and A. I. Mourikis, “Motion tracking with fixed-lag smoothing: Algorithm and consistency analysis,” in 2011 IEEE International Conference on Robotics and Automation. IEEE, 2011, pp. 5655–5662.
  23. [23] M. Ramezani, K. Khosoussi, G. Catt, P. Moghadam, J. Williams, P. Borges, F. Pauling, and N. Kottege, “Wildcat: Online continuous-time 3d lidar-inertial slam,” arXiv preprint arXiv:2205.12595, 2022.
  24. [24] M. Bosse, R. Zlot, and P. Flick, “Zebedee: Design of a spring-mounted 3-d range sensor with application to mobile mapping,” IEEE Transactions on Robotics, vol. 28, no. 5, pp. 1104–1119, 2012.
  25. [25] P. J. Huber, “Robust estimation of a location parameter,” in Breakthroughs in statistics: Methodology and distribution. Springer, 1992, pp. 492–518.
  26. [26] Z. Zhang and D. Scaramuzza, “A tutorial on quantitative trajectory evaluation for visual (-inertial) odometry,” in 2018 IEEE/RSJ international conference on intelligent robots and systems (IROS). IEEE, 2018, pp. 7244–7251.
  27. [27] M. Grupp, “evo: Python package for the evaluation of odometry and slam,” 2017.