On the Derivation of Tightly-Coupled LiDAR-Inertial Odometry with VoxelMap

Zhihao Zhan

cs.ROMar 16, 2026

On the Derivation of Tightly-Coupled LiDAR-Inertial Odometry with VoxelMap

Authors: Zhihao Zhan

Abstract

This note presents a concise mathematical formulation of tightly-coupled LiDAR-Inertial Odometry within an iterated error-state Kalman filter framework using a VoxelMap representation. Rather than proposing a new algorithm, it provides a clear and self-contained derivation that unifies the geometric modeling and probabilistic state estimation through consistent notation and explicit formulations. The document is intended to serve both as a technical reference and as an accessible entry point for a foundational understanding of the system architecture and estimation principles.

Figures & tables

Figure 1: System overview of the VoxelMap-based LIO framework. LiDAR and IMU measurements are preprocessed and fused within an iterated error-state Kalman filter. The estimated pose registers points to the global frame and incrementally updates the VoxelMap.

Symbols	Meaning
$t_{k}$	The scan-end time of the $k$ -th LiDAR scan.
$\tau_{i}$	The $i$ -th IMU sample time in a LiDAR scan.
$\rho_{j}$	The $j$ -th feature point’s sample time in a LiDAR scan.
$I_{i},I_{j},I_{k}$	The IMU body frame at the time $\tau_{i}$ , $\rho_{j}$ and $t_{k}$ .
$L_{j},L_{k}$	The LiDAR body frame at the time $\rho_{j}$ and $t_{k}$ .
$\mathbf{x},\widehat{\mathbf{x}},\bar{\mathbf{x}}$	The ground-true, propagated, and updated value of $\mathbf{x}$ .

Table 1: Important Notations.

Figure 2: The uncertainty model of the registered point.

Figure 3: The Uncertainty model of the plane normal.

Figure 4: The forward and backward propagation.

LiDAR Odometry

LiDAR odometry (light detection and ranging) estimates a robot’s motion by aligning successive laser scans, often with inertial data.

See all 46 papers

CT-VoxelMap: Efficient Continuous-Time LiDAR-Inertial Odometry with Probabilistic Adaptive Voxel Mapping
Apr 4, 2026Lei Zhao, Xingyi Li, Tianchen Deng +3B-Spline Trajectory Representation Adaptive Lidar-Inertial Odometry
2Fast-2Lamaa: Large-Scale Lidar-Inertial Localization and Mapping with Continuous Distance Fields
Oct 7, 2024Cedric Le Gentil, Raphael Falque, Daniil Lisus +1Simultaneous Localization and Mapping Inertial Odometry
Robotics Institute, University of Toronto Institute for Aerospace Studies, Canada · Robotics Institute, University of Technology Sydney, Australia
PA-LVIO: Real-Time LiDAR-Visual-Inertial Odometry and Mapping with Pose-Only Bundle Adjustment
Mar 17, 2026Hailiang Tang, Tisheng Zhang, Liqiang Wang +3Adaptive Lidar-Inertial Odometry Simultaneous Localization and Mapping
the GNSS Research Center, Wuhan University, Wuhan 430079, China · the Hubei Technology Innovation Center for Spatiotemporal Information and Positioning Navigation, Wuhan 430079, China · the Hubei Luojia Laboratory, Wuhan 430079, China

Apr 4, 2026J/K move · Enter open · S save

CT-VoxelMap: Efficient Continuous-Time LiDAR-Inertial Odometry with Probabilistic Adaptive Voxel Mapping

Lei Zhao, Xingyi Li, Tianchen Deng, Chuan Cao+2

B-Spline Trajectory Representation Adaptive Lidar-Inertial Odometry LiDAR Odometry Robot State Estimation Simultaneous Localization and Mapping

Open paper AskPDF arXiv

Maintaining stable and accurate localization during fast motion or on rough terrain remains highly challenging for mobile robots with onboard resources. Currently, multi-sensor fusion methods based on continuous-time representation offer a potential and effective solution to this challenge. Among these, spline-based methods provide an efficient and intuitive approach for continuous-time representation. Previous continuous-time odometry works based on B-splines either treat control points as variables to be estimated or perform estimation in quaternion space, which introduces complexity in deriving analytical Jacobians and often overlooks the fitting error between the spline and the true trajectory over time. To address these issues, we first propose representing the increments of control points on matrix Lie groups as variables to be estimated. Leveraging the feature of the cumulative form of B-splines, we derive a more compact formulation that yields simpler analytical Jacobians without requiring additional boundary condition considerations. Second, we utilize forward propagation information from IMU measurements to estimate fitting errors online and further introduce a hybrid feature-based voxel map management strategy, enhancing system accuracy and robustness. Finally, we propose a re-estimation policy that significantly improves system computational efficiency and robustness. The proposed method is evaluated on multiple challenging public datasets, demonstrating superior performance on most sequences. Detailed ablation studies are conducted to analyze the impact of each module on the overall pose estimation system.

Abstract

LiDAR Odometry

Explore similar work

CT-VoxelMap: Efficient Continuous-Time LiDAR-Inertial Odometry with Probabilistic Adaptive Voxel Mapping

2Fast-2Lamaa: Large-Scale Lidar-Inertial Localization and Mapping with Continuous Distance Fields

PA-LVIO: Real-Time LiDAR-Visual-Inertial Odometry and Mapping with Pose-Only Bundle Adjustment