1 Introduction↩︎

Accurate and real-time state estimation is fundamental to autonomous systems operating under highly dynamic conditions, such as autonomous racing [1], [2], aggressive autonomous driving [3], [4], and high-speed aerial navigation [5]. Modern LiDAR-inertial odometry has become the dominant solution for this task by combining the geometric accuracy of LiDAR [6] with the high temporal resolution of inertial measurements [7]–[9].

In most existing LIO systems, high-rate IMU measurements are first integrated to propagate the platform trajectory, and the resulting trajectory is subsequently used to de-distort each LiDAR scan before scan-to-map registration [10], [11]. Although this tightly coupled pipeline is computationally efficient and performs well in ordinary driving scenarios, it also causes inertial measurement noise to be directly propagated into the corrected point cloud. As a result, the quality of LiDAR registration becomes inherently dependent on the quality of IMU propagation. This issue becomes particularly pronounced during highly dynamic maneuvers, where severe vehicle vibration contaminates inertial measurements [12], [13] while rapid platform motion simultaneously demands highly accurate scan de-distortion.

A direct way to alleviate this coupling is to avoid relying on IMU-driven trajectory propagation and instead reconstruct the scan trajectory using an explicit motion model. Early approaches [6], [14], [15] employ low-order kinematic assumptions, typically constant linear/angular velocity [15], to predict the sensor motion during a scan. More recent continuous-time methods adopt higher-order motion priors to obtain more expressive trajectory representations, including GP models based on the WNOJ prior [16], the Singer motion model [17], and hybrid formulations that assume constant acceleration for translation and constant angular velocity for rotation [18]. These approaches improve trajectory reconstruction without relying on raw IMU integration. However, relying solely on a motion prior inevitably sacrifices the high-frequency motion constraints provided by inertial measurements.

Rather than replacing IMU measurements entirely, a more attractive solution is to combine expressive motion priors with high-frequency inertial constraints. Recent continuous-time LiDAR-inertial systems have demonstrated that GP motion priors provide smooth trajectory interpolation, improved motion compensation, and principled IMU preintegration [19], [20]. Meanwhile, recursive EKF-based methods, represented by FAST-LIO and its variants, have achieved remarkable accuracy, robustness, and computational efficiency for real-time LiDAR-IMU fusion, establishing the EKF as the dominant front-end architecture in modern LIO systems. This naturally motivates combining higher-order motion priors with recursive EKF estimation, allowing the motion model to provide physically consistent trajectory prediction while retaining the high-rate correction capability of inertial measurements.

Despite this appealing perspective, existing higher-order motion priors are almost exclusively formulated for continuous-time optimization rather than recursive Bayesian filtering. In particular, the classical continuous-time model, such as WNOJ, represents the system state on \(\mathrm{SE}(3)\), where translational and rotational motions are tightly coupled and the associated covariance is defined only in the local pose space. These formulations are therefore incompatible with the recursive covariance propagation and state-update mechanism of the EKF, leaving the integration of higher-order motion priors into EKF-based LiDAR-IMU fusion largely unexplored.

To address this limitation, this paper proposes WNOJ-LIO, a LiDAR-IMU fusion framework based on a WNOJ EKF for high-dynamic state estimation. Instead of using the IMU as the driver of state propagation, the proposed framework employs a decoupled WNOJ prior on \(\mathbb{R}^3 \times \mathrm{SO}(3)\) to predict the system state, while treating IMU measurements as high-frequency EKF updates. The posterior state history generated by the recursive IMU updates is subsequently used for LiDAR scan de-distortion and point-to-plane registration. This architecture enables the motion prior to suppress vibration-induced inertial disturbances while allowing IMU measurements to continuously correct the predicted trajectory, thereby combining the complementary strengths of model-based prediction and measurement-driven estimation. The resulting process model admits closed-form covariance propagation, providing a principled bridge between continuous-time GP motion priors and recursive EKF-based LiDAR-IMU fusion. The overall framework is illustrated in Fig. 1.

The main contributions of this paper are:

1. A decoupled WNOJ process model on \(\mathbb{R}^3 \times \mathrm{SO}(3)\) that enables recursive EKF filtering through closed-form state and covariance propagation, thereby making higher-order GP motion priors compatible with recursive LiDAR-IMU fusion.

2. A dual-frequency LiDAR-IMU fusion architecture that treats IMU measurements as EKF updates rather than the prediction driver, reducing the propagation of vibration-induced inertial disturbances into LiDAR scan de-distortion through a higher-order WNOJ motion prior.

3. Extensive simulation and real-world experiments on autonomous racing demonstrate improved inertial-state estimation, body-frame velocity, attitude estimation, and competitive localization under highly dynamic motion.

The remainder of this paper is organized as follows. Section 2 reviews related work. Section 3 formulates the problem. Section 4 presents the decoupled WNOJ process model, including local variables, global-state prediction, and covariance propagation. Section 5 instantiates this process model in a WNOJ-LIO-based EKF with augmented states, IMU update, de-distortionand, and LiDAR updates. Sections 6 and 7 present simulation and racing-car experiments, respectively. Section 8 concludes the paper and summarizes limitations.

2 Related Work↩︎

2.1 Continuous-Time State Estimation and WNOJ Motion Priors↩︎

Continuous-time state estimation provides a natural framework for representing platform motion during a LiDAR scan and for fusing asynchronous sensor measurements. Rather than estimating a single pose for each scan, it models the platform trajectory as a continuous function of time, allowing poses to be queried at arbitrary point timestamps. Existing continuous-time methods can generally be divided into parametric and nonparametric trajectory representations. Parametric approaches typically represent the trajectory using B-splines [21]–[26] or piecewise linear interpolation [27]–[29]. In contrast, nonparametric approaches formulate the trajectory as a Gaussian process, leading to exactly sparse continuous-time estimation on \(\mathrm{SE}(3)\) [16], [17], [30] and its extensions to matrix Lie groups [31]. These formulations have demonstrated the advantages of continuous-time trajectories for motion compensation and asynchronous multi-sensor fusion.

Linear interpolation offers the lowest computational cost but relies on piecewise constant-velocity assumptions, which become less accurate during aggressive maneuvers. To improve trajectory fidelity, several methods increase the density of trajectory control poses [28] or adopt higher-order spline representations [25].

In contrast, GP-based formulations explicitly model system kinematics through stochastic differential equations, producing trajectory priors whose state variables correspond directly to the kinematic quantities required for autonomous estimation and control. Within GP-based continuous-time estimation, the choice of motion prior determines both the system state and the assumed trajectory smoothness. The White-Noise-on-Acceleration (WNOA) prior models constant generalized acceleration perturbed by white noise [30], whereas the WNOJ prior extends the system to a third-order stochastic process on local pose by explicitly modeling jerk [16]. Data-driven Singer priors further learn motion statistics from trajectory datasets [17]. However, existing GP-prior-based LiDAR-inertial systems [20], [32] do not directly offer the same prediction-update structure and runtime profile as EKF-based LIO.

2.2 LiDAR-Inertial Fusion↩︎

Although LiDAR-only odometry has achieved remarkable performance, high-rate inertial measurements are widely incorporated to improve state estimation and provide motion information for LiDAR scan de-distortion. Existing LiDAR–inertial odometry systems can be broadly categorized into optimization-based and filtering-based frameworks according to how IMU measurements are processed.

Optimization-based methods, such as LIO-SAM [11] and LIC-Fusion [33], typically process IMU measurements in two stages. First, raw inertial measurements are propagated forward to provide an initial trajectory for scan de-distortion and optimization initialization. Subsequently, IMU preintegration is formulated as a factor to constrain the optimized trajectory within a graph or sliding window. Continuous-time optimization methods, including CLINS, SLICT, and Coco-LIC [21], [24], [25], follow a similar strategy while replacing the discrete trajectory with a continuous-time representation and introducing continuous-time motion priors into the optimization.

Filtering-based methods provide a different trade-off by maintaining a fixed-size state and processing measurements recursively. Representative systems such as FAST-LIO and FAST-LIO2 tightly couple IMU propagation with LiDAR residuals within an iterated Extended Kalman Filter (IEKF), achieving excellent computational efficiency and real-time performance [10], [34]. In these methods, however, the IMU serves as the process model driving state prediction, and the propagated trajectory is directly used for scan de-distortion.

More recently, continuous-time LiDAR–inertial systems have explored an alternative formulation in which IMU measurements are treated as observations of the continuous-time trajectory rather than as inputs to the process model [20], [35]. This formulation enables the motion prior to estimate the entire trajectory while allowing inertial measurements to constrain the underlying kinematic states. Nevertheless, these methods rely on sliding-window optimization, where a large number of trajectory states associated with high-rate IMU measurements must be optimized simultaneously, resulting in substantially higher computational cost than EKF-based methods. Consequently, integrating continuous-time motion priors with the computational efficiency of recursive EKF-based LiDAR-IMU fusion remains largely unexplored.

3pt

3 Problem Formulation and Preliminaries↩︎

3.1 Notation and Coordinate Conventions↩︎

We describe platform motion with position, velocity, acceleration, rotation, angular velocity, and angular acceleration. These quantities are denoted by \(\mathbf{p}\), \(\mathbf{v}\), \(\mathbf{a}\), \(\mathbf{R}\), \(\boldsymbol{\omega}\), and \(\boldsymbol{\alpha}\) when no coordinate-frame ambiguity exists. Two coordinate frames are used throughout the paper: the world frame \(w\) and the platform body frame \(b\). A superscript indicates the frame in which a vector is expressed, and a two-letter subscript indicates the physical quantity or transformation from the second frame to the first frame. For example, \(\mathbf{p}^w_{wb}\) is the position of the body frame relative to the world frame, expressed in \(w\), and \(\mathbf{R}_{wb}\) maps body-frame vectors into the world frame. In the LiDAR-IMU fusion context, the body frame coincides with the LiDAR frame, and the IMU frame is related via a known extrinsic calibration.

We use standard Lie-group notation for rotations and rigid-body poses. The rotation matrix satisfies \(\mathbf{R}\in\mathrm{SO}(3)\), where \(\mathrm{SO}(3)\) is the three-dimensional rotation group, and \(\mathfrak{so}(3)\) denotes its Lie algebra. The rigid-body pose is represented by \(\mathbf{T}=\begin{bmatrix}\mathbf{R}&\mathbf{p}\\ \mathbf{0}&1\end{bmatrix}\in\mathrm{SE}(3)\), where \(\mathrm{SE}(3)\) is the special Euclidean group and \(\mathfrak{se}(3)\) is its Lie algebra. A local pose perturbation on \(\mathrm{SE}(3)\) is denoted by \(\boldsymbol{\xi}=[\boldsymbol{\rho}^\top,\boldsymbol{\phi}^\top]^\top\in\mathbb{R}^6\), where \(\boldsymbol{\rho}\) is the translational perturbation and \(\boldsymbol{\phi}\) is the rotational perturbation. The hat operator \(\left[\cdot\right]^\wedge\) maps a vector to a skew-symmetric matrix. The operators \(\exp(\cdot)\) and \(\log(\cdot)\) are used for both \(\mathrm{SO}(3)\) and \(\mathrm{SE}(3)\). For rotations, \(\exp: \mathfrak{so}(3) \to \mathrm{SO}(3)\) recovers a rotation matrix from its Lie algebra element, and \(\log: \mathrm{SO}(3) \to \mathfrak{so}(3)\) is the inverse. The right Jacobian \(\mathbf{J}_r(\boldsymbol{\phi})\) and its inverse satisfy the first-order approximations [36] \[\mathbf{J}_r(\boldsymbol{\phi}) \approx \mathbf{I}- \tfrac{1}{2}\left[\boldsymbol{\phi}\right]^\wedge, \quad \mathbf{J}_r^{-1}(\boldsymbol{\phi}) \approx \mathbf{I}+ \tfrac{1}{2}\left[\boldsymbol{\phi}\right]^\wedge, \label{eq:jr95approx}\tag{1}\] valid for small \(\|\boldsymbol{\phi}\|\).

3.2 Gaussian Process Kinematic Priors↩︎

Many continuous-time trajectory estimation methods employ GP priors represented by linear time-invariant (LTI) stochastic differential equations (SDEs) [16] \[\dot{\boldsymbol{\gamma}}(t) = \mathbf{A}\,\boldsymbol{\gamma}(t) + \mathbf{L}\,\mathbf{w}(t), \quad \mathbf{w}(t) \sim \mathcal{GP}(\mathbf{0}, \mathbf{Q}_c\,\delta(t-\tau)), \label{eq:gp95sde}\tag{2}\] where \(\boldsymbol{\gamma}(t)\) is the state, \(\mathbf{w}(t)\) is a white-noise GP with power spectral density \(\mathbf{Q}_c\), and \(\delta(\cdot)\) is the Dirac delta. The prior mean propagates via the state transition function \(\boldsymbol{\Phi}\) according to \[\hat{\boldsymbol{\gamma}}(\tau) = \boldsymbol{\Phi}(\tau, t_k)\,\hat{\boldsymbol{\gamma}}(t_k), \quad \boldsymbol{\Phi}(\tau, t_k) = \exp(\mathbf{A}\,\Delta t),\] where \(\Delta t = \tau - t_k\).

3.3 Classical WNOJ on \(\mathrm{SE}(3)\) and Its Limitations↩︎

The classical WNOJ model [16] represents the trajectory on \(\mathrm{SE}(3)\) as \[\mathbf{T}(t) = \exp(\boldsymbol{\xi}^\wedge) = \begin{bmatrix} \mathbf{R}(t) & \mathbf{p}(t) \\ \mathbf{0}& 1 \end{bmatrix} \in \mathrm{SE}(3),\] with the global state \(\{\mathbf{T}(t), \boldsymbol{\varpi}(t), \dot{\boldsymbol{\varpi}}(t)\}\), where \(\boldsymbol{\varpi}(t) \in \mathbb{R}^6\) is the body-frame generalized velocity and \(\dot{\boldsymbol{\varpi}}(t) \in \mathbb{R}^6\) is the body-frame generalized acceleration. Since the pose kinematics \(\dot{\mathbf{T}} = \boldsymbol{\varpi}^\wedge\mathbf{T}\) is nonlinear, we define a local pose variable as \[\delta\boldsymbol{\xi}_i(t) := \log\!\left(\mathbf{T}(t)\,\mathbf{T}_i^{-1}\right), \quad t \in [t_i, t_{i+1}),\] where \(\mathbf{T}_i = \mathbf{T}(t_i)\). The augmented local state \(\boldsymbol{\gamma}_i(t) = {[\delta\boldsymbol{\xi}_i^\top,\; \dot{\delta\boldsymbol{\xi}}_i^\top,\; \ddot{\delta\boldsymbol{\xi}}_i^\top]}^\top\) satisfies the LTI SDE in Eq. 2 with \[\mathbf{A} = \begin{bmatrix} \mathbf{0}& \mathbf{I}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{I}\\ \mathbf{0}& \mathbf{0}& \mathbf{0} \end{bmatrix}_{18\times18}, \quad \mathbf{L} = \begin{bmatrix} \mathbf{0}\\ \mathbf{0}\\ \mathbf{I}\end{bmatrix}_{18\times6},\] meaning the jerk \(\dddot{\boldsymbol{\xi}}_i(t) = \mathbf{w}(t)\) is white noise. The local state transition matrix is given by \[\boldsymbol{\Phi}(t_{i+1}, t_i) = \begin{bmatrix} \mathbf{I}& \Delta t\,\mathbf{I}& \frac{1}{2}\Delta t^2\,\mathbf{I}\\ \mathbf{0}& \mathbf{I}& \Delta t\,\mathbf{I}\\ \mathbf{0}& \mathbf{0}& \mathbf{I} \end{bmatrix}_{18\times18}, \label{eq:Phi95classical}\tag{3}\] and the process noise covariance follows from the standard GP integral.

Although the conventional WNOJ formulation is not directly amenable to EKF-based estimation because of the coupling between translational and rotational states and its local covariance propagation, it provides the high-order kinematic variables required for highly dynamic LiDAR–inertial fusion, including pose, linear velocity and acceleration, and angular velocity and acceleration. In the following section, we reformulate the WNOJ prior by decoupling the translational and rotational dynamics and propagating the covariance in the global state space, thereby rendering it suitable as the prediction model of WNOJ-LIO.

4 Decoupled WNOJ Process Model↩︎

In this section, we first derive the decoupled kinematic differential equations, then construct local pose variables and the corresponding LTI SDE. We finally map the local WNOJ recursion back to the global state and derive the covariance propagation needed by an EKF.

4.1 Kinematic Decoupling via Coordinate Selection↩︎

The key insight is that the coupling between position and attitude derivatives depends on the choice of reference frame for the velocity variables. Using the Poisson equation, the attitude kinematics takes the form \[\dot{\mathbf{R}}_{wb} = \mathbf{R}_{wb}\,\left[\boldsymbol{\omega}^b_{wb}\right]^\wedge, \label{eq:poisson}\tag{4}\] which is independent of position. Expressing the velocity in a rotating body frame gives the position kinematics \[\dot{\mathbf{p}}^w_{wb} = -\mathbf{R}_{wb}\,\left[\mathbf{p}^b_{wb}\right]^\wedge\,\boldsymbol{\omega}^b_{wb} + \mathbf{R}_{wb}\,\mathbf{v}^b_{wb},\] which depends on \(\boldsymbol{\omega}^b_{wb}\), \(\mathbf{R}_{wb}\), and \(\mathbf{v}^b_{wb}\), creating a strong coupling. To decouple, we instead express velocity in the world frame and obtain the fully decoupled system \[\dot{\mathbf{p}}^w_{wb}(t) = \mathbf{v}^w_{wb}(t), \qquad \dot{\mathbf{R}}_{wb}(t) = \mathbf{R}_{wb}(t)\,\left[\boldsymbol{\omega}^b_{wb}(t)\right]^\wedge. \label{eq:decoupled95diff}\tag{5}\] Introducing the acceleration variable \(\mathbf{a}^w_{wb}\) and angular-acceleration variable \(\boldsymbol{\alpha}^b_{wb}\) and imposing the WNOJ prior gives the continuous-time system \[\begin{align} \dot{\mathbf{p}}^w_{wb}(t) &= \mathbf{v}^w_{wb}(t), \\ \dot{\mathbf{v}}^w_{wb}(t) &= \mathbf{a}^w_{wb}(t), \\ \dot{\mathbf{a}}^w_{wb}(t) &= \mathbf{w}^w_{\mathbf{a}}(t), \quad \mathbf{w}^w_{\mathbf{a}} \sim \mathcal{GP}(\mathbf{0}, \mathbf{q}_{\mathbf{a}}\,\delta(t-\tau)), \\ \dot{\mathbf{R}}_{wb}(t) &= \mathbf{R}_{wb}(t)\,\left[\boldsymbol{\omega}^b_{wb}(t)\right]^\wedge, \\ \dot{\boldsymbol{\omega}}^b_{wb}(t) &= \boldsymbol{\alpha}^b_{wb}(t), \\ \dot{\boldsymbol{\alpha}}^b_{wb}(t) &= \mathbf{w}^b_{\boldsymbol{\alpha}}(t), \quad \mathbf{w}^b_{\boldsymbol{\alpha}} \sim \mathcal{GP}(\mathbf{0}, \mathbf{q}_{\boldsymbol{\alpha}}\,\delta(t-\tau)). \end{align} \label{eq:wnoj95continuous}\tag{6}\] Here, \(\mathbf{p}^w_{wb}\), \(\mathbf{v}^w_{wb}\), and \(\mathbf{a}^w_{wb}\) are expressed in the world frame, while \(\boldsymbol{\omega}^b_{wb}\) and \(\boldsymbol{\alpha}^b_{wb}\) are expressed in the body frame. The translational jerk noise \(\mathbf{w}^w_{\mathbf{a}}\) is therefore defined in the world frame, and the angular-acceleration noise \(\mathbf{w}^b_{\boldsymbol{\alpha}}\) is defined in the body frame. For notational convenience, the frame superscripts and subscripts are omitted in the following derivations unless they are needed for clarity. The position and attitude subsystems are fully decoupled: position kinematics lives entirely in \(\mathbb{R}^3\) and attitude kinematics on \(\mathrm{SO}(3)\).

4.2 Local Pose Variables and the Decoupled LTI SDE↩︎

The decoupled global model above separates translation and rotation, but the EKF still requires a vector-space covariance recursion. We therefore introduce local pose variables over each propagation interval. This construction keeps the WNOJ prior linear in the tangent space while preserving the global-state prediction on \(\mathbb{R}^3 \times \mathrm{SO}(3)\). Since \(\mathbf{R} \in \mathrm{SO}(3)\) is not a vector-space quantity, we linearize in the local tangent space. For the time interval \([t_i, t_{i+1})\), we define the local pose variables as \[\delta\mathbf{p}_i(t) = \mathbf{p}(t) - \mathbf{p}(t_i), \qquad \delta\boldsymbol{\phi}_i(t) = \log\!\left(\mathbf{R}_i^{-1}\,\mathbf{R}(t)\right), \label{eq:local95vars}\tag{7}\] where \(\mathbf{R}_i := \mathbf{R}(t_i)\). Differentiation yields the local velocities \[\dot{\delta\mathbf{p}}_i(t) = \mathbf{v}(t), \qquad \dot{\delta\boldsymbol{\phi}}_i(t) = \mathbf{J}_r\!\left(\delta\boldsymbol{\phi}_i(t)\right)^{-1}\boldsymbol{\omega}(t). \label{eq:local95velocity}\tag{8}\] Using the first-order approximation as Eq. 1 , the local angular acceleration is approximated as \[\ddot{\delta\boldsymbol{\phi}}_i(t) \approx \tfrac{1}{2}\left[\dot{\delta\boldsymbol{\phi}}_i(t)\right]^\wedge\,\boldsymbol{\omega}(t) + \mathbf{J}_r^{-1}\!\left(\delta\boldsymbol{\phi}_i(t)\right)\boldsymbol{\alpha}(t). \label{eq:local95angular95acc}\tag{9}\]

The augmented local state is partitioned as \[\boldsymbol{\gamma}_i(t) = \begin{bmatrix} \boldsymbol{\gamma}_{\mathbf{p},i}(t) \\ \boldsymbol{\gamma}_{\boldsymbol{\phi},i}(t) \end{bmatrix}, \quad \boldsymbol{\gamma}_{\mathbf{p},i} = \begin{bmatrix} \delta\mathbf{p}_i \\ \dot{\delta\mathbf{p}}_i \\ \ddot{\delta\mathbf{p}}_i \end{bmatrix}, \quad \boldsymbol{\gamma}_{\boldsymbol{\phi},i} = \begin{bmatrix} \delta\boldsymbol{\phi}_i \\ \dot{\delta\boldsymbol{\phi}}_i \\ \ddot{\delta\boldsymbol{\phi}}_i \end{bmatrix}. \label{eq:gamma95def}\tag{10}\] Under the decoupled WNOJ prior, the \(\dddot{\delta\mathbf{p}}_i\) and \(\dddot{\delta\boldsymbol{\phi}}_i\) are modeled as independent white-noise Gaussian processes with power spectral density \(\mathbf{q}_{\mathbf{p}}\) and \(\mathbf{q}_{\boldsymbol{\phi}}\), respectively. Specifically, \[\mathbf{w}(t) = \begin{bmatrix} \mathbf{w}_{\mathbf{p}}(t) \\ \mathbf{w}_{\boldsymbol{\phi}}(t) \end{bmatrix}, \qquad \begin{align} \mathbf{w}_{\mathbf{p}}(t) &\sim \mathcal{GP}\left( \mathbf{0}, \mathbf{q}_{\mathbf{p}}\delta(t-\tau) \right), \\ \mathbf{w}_{\boldsymbol{\phi}}(t) &\sim \mathcal{GP}\left( \mathbf{0}, \mathbf{q}_{\boldsymbol{\phi}}\delta(t-\tau) \right). \end{align}\]

The resulting LTI SDE exhibits the block-diagonal structure \[\mathbf{A} = \begin{bmatrix} \mathbf{A}_{\mathbf{p}} & \mathbf{0}_{9\times9} \\ \mathbf{0}_{9\times9} & \mathbf{A}_{\boldsymbol{\phi}} \end{bmatrix}, \quad \mathbf{L} = \begin{bmatrix} \mathbf{L}_{\mathbf{p}} & \mathbf{0}_{9\times3} \\ \mathbf{0}_{9\times3} & \mathbf{L}_{\boldsymbol{\phi}} \end{bmatrix}, \label{eq:decoupled95AL}\tag{11}\] where \(\mathbf{A}_{\mathbf{p}} = \mathbf{A}_{\boldsymbol{\phi}}\) and \(\mathbf{L}_{\mathbf{p}} = \mathbf{L}_{\boldsymbol{\phi}}\) have the same form as the classical WNOJ matrices given by \[\mathbf{A}_{\mathbf{p}} = \begin{bmatrix} \mathbf{0}& \mathbf{I}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{I}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}\end{bmatrix}_{9\times9}, \quad \mathbf{L}_{\mathbf{p}} = \begin{bmatrix} \mathbf{0}\\ \mathbf{0}\\ \mathbf{I}\end{bmatrix}_{9\times3}.\]

4.3 Discrete-Time System Equations↩︎

The continuous-time local SDE is used over each finite propagation interval. We next discretize this local model over \([t_i,t_{i+1}]\) and relate the initialized local variables to the global state, so that the resulting mean and covariance can be used in the EKF prediction step.

The local state transition from \(t_i\) to \(t_{i+1}\) is expressed as \[\boldsymbol{\gamma}_i(t_{i+1}) = \boldsymbol{\Phi}(t_{i+1},t_i)\,\boldsymbol{\gamma}_i(t_i) + \mathbf{w}_{t_i}, \label{eq:discrete95local}\tag{12}\] where the transition matrix takes the following block-diagonal form \[\boldsymbol{\Phi}(t_{i+1},t_i) = \begin{bmatrix} \boldsymbol{\Phi}_{\mathbf{p}} & \mathbf{0}_{9\times9} \\ \mathbf{0}_{9\times9} & \boldsymbol{\Phi}_{\boldsymbol{\phi}} \end{bmatrix}, \label{eq:Phi95decoupled}\tag{13}\] with \(\boldsymbol{\Phi}_{\mathbf{p}} = \boldsymbol{\Phi}_{\boldsymbol{\phi}}\) having the same form as Eq. 3 but at the \(9\times9\) level. \(\mathbf{w}_{t_i} = \int_{t_i}^{t_{i+1}} \boldsymbol{\Phi}(t_{i+1},\tau)\mathbf{L}\mathbf{w}(\tau)\,d\tau\).

At the initial time \(t_i\), the local pose variables satisfy \(\delta\mathbf{p}_i(t_i) = \mathbf{0}\), \(\delta\boldsymbol{\phi}_i(t_i) = \mathbf{0}\), and \(\mathbf{J}_r(\mathbf{0}) = \mathbf{I}\). It follows that \[\boldsymbol{\gamma}_i(t_i) = \begin{bmatrix} \mathbf{0}\\ \mathbf{v}(t_i) \\ \mathbf{a}(t_i) \\ \hline \mathbf{0}\\ \boldsymbol{\omega}(t_i) \\ \boldsymbol{\alpha}(t_i) \end{bmatrix}, \quad \mathbf{P}_{\boldsymbol{\gamma}_i(t_i)} = \mathop{\mathrm{diag}}\!\left(\mathbf{0}_3,\,\mathbf{P}_{\mathbf{v}},\,\mathbf{P}_{\mathbf{a}},\,\mathbf{0}_3,\,\mathbf{P}_{\boldsymbol{\omega}},\,\mathbf{P}_{\boldsymbol{\alpha}}\right). \label{eq:gamma95initial}\tag{14}\]

The local-state covariance propagates according to \[\mathbf{P}_{\boldsymbol{\gamma}_i(t_{i+1})} = \boldsymbol{\Phi}(t_{i+1},t_i)\,\mathbf{P}_{\boldsymbol{\gamma}_i(t_i)}\,\boldsymbol{\Phi}(t_{i+1},t_i)^\top+ \mathbf{Q}_{t_i}, \label{eq:cov95local}\tag{15}\] where \(\mathbf{Q}_{t_i} = \int_{t_i}^{t_{i+1}} \boldsymbol{\Phi}(t_{i+1},\tau) \mathbf{L}\mathbf{Q}_c\mathbf{L}^{\top} \boldsymbol{\Phi}(t_{i+1},\tau)^{\top} \mathrm{d}\tau\), \(\mathbf{Q}_c = \mathop{\mathrm{diag}}\left( \mathbf{q}_{\mathbf{p}}, \mathbf{q}_{\boldsymbol{\phi}} \right)\). Because the translational and rotational jerk processes are independent, the discrete-time process-noise covariance takes the block-diagonal form \[\mathbf{Q}_{t_i} = \begin{bmatrix} \mathbf{Q}_{\mathbf{p},t_i} & \mathbf{0}_{9\times9} \\ \mathbf{0}_{9\times9} & \mathbf{Q}_{\boldsymbol{\phi},t_i} \end{bmatrix}, \label{eq:local95Q95blocks}\tag{16}\] where both covariance blocks share the coefficient matrix \[\mathbf{C}_{i} = \begin{bmatrix} \frac{\Delta t^5}{20} & \frac{\Delta t^4}{8} & \frac{\Delta t^3}{6} \\ \frac{\Delta t^4}{8} & \frac{\Delta t^3}{3} & \frac{\Delta t^2}{2} \\ \frac{\Delta t^3}{6} & \frac{\Delta t^2}{2} & \Delta t \end{bmatrix}. \label{eq:wnoj95cov95coeff}\tag{17}\] The translational and rotational components are then \[\mathbf{Q}_{\mathbf{p},t_i} = \mathbf{C}_{i}\otimes\mathbf{q}_{\mathbf{p}}, \qquad \mathbf{Q}_{\boldsymbol{\phi},t_i} = \mathbf{C}_{i}\otimes\mathbf{q}_{\boldsymbol{\phi}}, \label{eq:local95Q95components}\tag{18}\] where \(\otimes\) denotes the Kronecker product.

Thus, translational and rotational process noise are injected through the local WNOJ acceleration and angular-acceleration channels. The global EKF covariance is obtained later by mapping this local covariance through the global-state Jacobian.

4.4 Global WNOJ State and Manifold Structure↩︎

The base WNOJ state at time \(t_i\) is defined on the composite manifold as \[\mathbf{X}_i = \{\mathbf{p}_i,\, \mathbf{v}_i,\, \mathbf{a}_i,\, \mathbf{R}_i,\, \boldsymbol{\omega}_i,\, \boldsymbol{\alpha}_i\} \in \mathcal{M} = \mathbb{R}^9 \times \mathrm{SO}(3) \times \mathbb{R}^6, \label{eq:state95def}\tag{19}\] with \(\dim(\mathcal{M}) = 18\). The \(\boxplus\) operator applies a tangent-space perturbation according to \[\mathbf{X} \boxplus \delta\mathbf{X} = \begin{bmatrix} \mathbf{p} + \delta\mathbf{p} \\ \mathbf{v} + \delta\mathbf{v} \\ \mathbf{a} + \delta\mathbf{a} \\ \mathbf{R}\,\exp(\delta\boldsymbol{\phi}) \\ \boldsymbol{\omega} + \delta\boldsymbol{\omega} \\ \boldsymbol{\alpha} + \delta\boldsymbol{\alpha} \end{bmatrix}. \label{eq:boxplus}\tag{20}\] The covariance is represented in the tangent space \(\mathbb{R}^{18}\), with the attitude block parameterized in \(\mathfrak{so}(3)\) via \(\boldsymbol{\phi} = \log(\mathbf{R})\).

4.5 Global State Prediction with WNOJ Prior↩︎

The local WNOJ model in Subsection 4.3 provides the relationship between the local state \(\boldsymbol{\gamma}_i(t)=\left[\delta\mathbf{p}_i^\top,\dot{\delta\mathbf{p}}_i^\top,\ddot{\delta\mathbf{p}}_i^\top,\delta\boldsymbol{\phi}_i^\top,\dot{\delta\boldsymbol{\phi}}_i^\top,\ddot{\delta\boldsymbol{\phi}}_i^\top\right]^\top\) and the global state at \(t_i\). Substituting Eq. 14 into Eq. 12 yields the predicted local state \[\boldsymbol{\gamma}_i(t_{i+1}) = \begin{bmatrix} \mathbf{v}(t_i)\Delta t + \frac{1}{2}\mathbf{a}(t_i)\Delta t^2 \\ \mathbf{v}(t_i) + \mathbf{a}(t_i)\Delta t \\ \mathbf{a}(t_i) \\ \hline \boldsymbol{\omega}(t_i)\Delta t + \frac{1}{2}\boldsymbol{\alpha}(t_i)\Delta t^2 \\ \boldsymbol{\omega}(t_i) + \boldsymbol{\alpha}(t_i)\Delta t \\ \boldsymbol{\alpha}(t_i) \end{bmatrix}. \label{eq:gamma95predict}\tag{21}\]

The global state prediction is obtained by combining Eq. 21 with the relationship between local and global variables. For the position part, this gives \[\begin{align} \mathbf{p}(t_{i+1}) &= \mathbf{p}(t_i) + \mathbf{v}(t_i)\Delta t + \tfrac{1}{2}\mathbf{a}(t_i)\Delta t^2, \\ \mathbf{v}(t_{i+1}) &= \mathbf{v}(t_i) + \mathbf{a}(t_i)\Delta t, \\ \mathbf{a}(t_{i+1}) &= \mathbf{a}(t_i). \end{align} \label{eq:pos95predict}\tag{22}\]

For the attitude part, the local attitude increment is given by \[\delta\boldsymbol{\phi}_i(t_{i+1}) = \boldsymbol{\omega}(t_i)\Delta t + \tfrac{1}{2}\boldsymbol{\alpha}(t_i)\Delta t^2, \label{eq:delta95phi95predict}\tag{23}\] and the rotation matrix is predicted as \[\mathbf{R}(t_{i+1}) = \mathbf{R}(t_i)\,\exp\!\left(\boldsymbol{\omega}(t_i)\Delta t + \tfrac{1}{2}\boldsymbol{\alpha}(t_i)\Delta t^2\right). \label{eq:R95predict}\tag{24}\]

For the angular velocity, combining \(\dot{\delta\boldsymbol{\phi}}_i(t_{i+1}) = \mathbf{J}_r^{-1}(\delta\boldsymbol{\phi}_i(t_{i+1}))\,\boldsymbol{\omega}(t_{i+1})\) with the first-order approximation gives \[\begin{gather} \boldsymbol{\omega}(t_{i+1}) = \mathbf{J}_r\!\left(\delta\boldsymbol{\phi}_i(t_{i+1})\right) \left(\boldsymbol{\omega}(t_i) + \boldsymbol{\alpha}(t_i)\Delta t\right) \\ \approx \boldsymbol{\omega}(t_i) + \boldsymbol{\alpha}(t_i)\Delta t - \tfrac{1}{2}\left[\boldsymbol{\omega}(t_i)\right]^\wedge\boldsymbol{\alpha}(t_i)\Delta t^2 \\ - \tfrac{1}{4}\left[\boldsymbol{\alpha}(t_i)\right]^\wedge\boldsymbol{\omega}(t_i)\Delta t^2. \label{eq:omega95predict} \end{gather}\tag{25}\]

For angular acceleration, the local relationship is \(\ddot{\delta\boldsymbol{\phi}}_i(t_{i+1})=\boldsymbol{\alpha}(t_i)\) under the WNOJ prior, while \(\ddot{\delta\boldsymbol{\phi}}_i(t_{i+1}) \approx \mathbf{J}_r^{-1}(\delta\boldsymbol{\phi}_i)\boldsymbol{\alpha}(t_{i+1})\). Using the first-order approximation of \(\mathbf{J}_r\) gives the angular-acceleration prediction \[\boldsymbol{\alpha}(t_{i+1}) \approx \boldsymbol{\alpha}(t_i) - \tfrac{1}{2}\left[\boldsymbol{\omega}(t_i)\right]^\wedge\boldsymbol{\alpha}(t_i)\Delta t. \label{eq:alpha95predict}\tag{26}\]

Equations 22 26 constitute the WNOJ global-state process model. Note that the angular velocity and acceleration propagation include correction terms involving \(\left[\boldsymbol{\omega}\right]^\wedge\boldsymbol{\alpha}\) and \(\left[\boldsymbol{\alpha}\right]^\wedge\boldsymbol{\omega}\) that reflect the non-Euclidean geometry of \(\mathrm{SO}(3)\), arising from the first-order approximation of the right Jacobian.

4.6 Global Covariance Prediction↩︎

The covariance prediction consists of two independent contributions. The first propagates the posterior state uncertainty at \(t_i\) through the Jacobian of the deterministic global-state prediction. The second accounts for the uncertainty introduced by the WNOJ process noise over \([t_i,t_{i+1}]\). As derived in Subsection 4.3, \(\mathbf{w}_{t_i}\) is the integrated process-noise increment of the local endpoint state \(\boldsymbol{\gamma}_i(t_{i+1})\), with zero mean and covariance \(\mathbf{Q}_{t_i}\).

The covariance of rotation in global state is represented in the Lie algebra space \(\boldsymbol{\phi} = \log(\mathbf{R})\). Using the BCH linear approximation, \(\boldsymbol{\phi}(t_{i+1}) = \boldsymbol{\phi}(t_i) + \mathbf{J}_r^{-1}(\boldsymbol{\phi}_i)\left(\boldsymbol{\omega}(t_i)\Delta t + \frac{1}{2}\boldsymbol{\alpha}(t_i)\Delta t^2\right)\). Let \(\mathbf{x}\) denotes the global state vector \(\mathbf{x}(t) = {[\mathbf{p}^\top, \mathbf{v}^\top, \mathbf{a}^\top, \boldsymbol{\phi}^\top, \boldsymbol{\omega}^\top, \boldsymbol{\alpha}^\top]}^\top\in \mathbb{R}^{18}\). Combining the deterministic state prediction with the local process-noise increment gives the linearized error propagation \[\mathbf{x}(t_{i+1}) = \boldsymbol{\Phi}_{i+1}'\,\mathbf{x}(t_i) + \boldsymbol{\Psi}_{i+1}'\,\mathbf{w}_{t_i}, \label{eq:linearized95state}\tag{27}\] where the state transition Jacobian \(\boldsymbol{\Phi}_{i+1}'\) is \[{!}{\displaystyle \boldsymbol{\Phi}_{i+1}' = \begin{bmatrix} \mathbf{I}& \Delta t\,\mathbf{I}& \frac{1}{2}\Delta t^2\,\mathbf{I}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{I}& \Delta t\,\mathbf{I}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{I}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\[4pt] \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{I}& \Delta t\,\mathbf{J}_r^{-1} & \frac{1}{2}\Delta t^2\,\mathbf{J}_r^{-1} \\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \frac{1}{4}\Delta t^2\left[\boldsymbol{\alpha}\right]^\wedge & \mathbf{I}+ \frac{1}{4}\Delta t^2\left[\boldsymbol{\alpha}\right]^\wedge & \Delta t\,\mathbf{I}- \frac{1}{4}\Delta t^2\left[\boldsymbol{\omega}\right]^\wedge \\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \frac{1}{2}\Delta t\,\left[\boldsymbol{\alpha}\right]^\wedge & \mathbf{I}- \frac{1}{2}\Delta t\,\left[\boldsymbol{\omega}\right]^\wedge \end{bmatrix}}, \label{eq:Phi95prime}\tag{28}\] where all skew-symmetric terms are evaluated at \(\boldsymbol{\omega}(t_i)\) and \(\boldsymbol{\alpha}(t_i)\), and \(\mathbf{J}_r^{-1} = \mathbf{J}_r^{-1}(\boldsymbol{\phi}_i)\). The matrix \(\boldsymbol{\Psi}_{i+1}'\) directly maps the integrated local-state process noise into the global-state vector. It is defined as the local-to-global Jacobian and, under the same first-order approximation used in the state prediction, is given by \[{!}{\displaystyle \boldsymbol{\Psi}_{i+1}' \triangleq \frac{\partial\mathbf{x}(t_{i+1})}{\partial\boldsymbol{\gamma}_i(t_{i+1})^\top} \approx \begin{bmatrix} \mathbf{I}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{I}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{I}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\[4pt] \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{J}_r^{-1}(\boldsymbol{\phi}_i) & \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \tfrac{1}{2}\left[\dot{\delta\boldsymbol{\phi}}_i(t_{i+1})\right]^\wedge & \mathbf{J}_r\!\left(\delta\boldsymbol{\phi}_i(t_{i+1})\right) & \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \tfrac{1}{2}\left[\ddot{\delta\boldsymbol{\phi}}_i(t_{i+1})\right]^\wedge & \mathbf{0}& \mathbf{J}_r\!\left(\delta\boldsymbol{\phi}_i(t_{i+1})\right) \end{bmatrix}}, \label{eq:Psi95prime}\tag{29}\]

The EKF covariance prediction is then \[\check{\mathbf{P}}_{i+1} = \boldsymbol{\Phi}_{i+1}'\,\hat{\mathbf{P}}_i\,(\boldsymbol{\Phi}_{i+1}')^\top + \boldsymbol{\Psi}_{i+1}'\,\mathbf{Q}_{t_i}\,(\boldsymbol{\Psi}_{i+1}')^\top, \label{eq:cov95predict}\tag{30}\] where \(\hat{\mathbf{P}}_i\) is the posterior covariance at \(t_i\), and \(\check{\mathbf{P}}_{i+1}\) is the predicted covariance at \(t_{i+1}\). The matrices \(\boldsymbol{\Phi}_{i+1}'\) and \(\boldsymbol{\Psi}_{i+1}'\) are therefore the state transition and local-to-global process-noise mapping blocks used by the WNOJ-LIO EKF in the next section.

5 WNOJ-LIO-Based EKF for LiDAR-IMU Fusion↩︎

The decoupled WNOJ model defines the base motion prior, but a LiDAR-IMU estimator must also include sensor-specific states, measurement models, and update scheduling. This section instantiates the WNOJ process model in an EKF. The resulting WNOJ-LIO estimator augments the 18-dimensional WNOJ state when a sensor requires additional variables, predicts the WNOJ and extension blocks with a partitioned process model, and updates the augmented state with high-rate IMU and low-rate LiDAR measurements.

5.1 Augmented EKF State and Process Model↩︎

The WNOJ motion state in Eq. 19 is the base state shared by all sensor configurations. Additional variables required by a sensor model are collected in an extension vector \(\mathbf{x}_{\mathrm{ext}}\). The augmented estimator state is defined as \[\mathbf{X}^{+}_i = \{\mathbf{X}^{\mathrm{WNOJ}}_i,\,\mathbf{x}_{\mathrm{ext},i}\}, \qquad \mathbf{X}^{\mathrm{WNOJ}}_i \equiv \mathbf{X}_i . \label{eq:aug95state95def}\tag{31}\] In the real-data WNOJ-LIO implementation, the IMU model requires the gravity vector. The nominal gravity is stored as a three-dimensional vector \(\mathbf{g}\in\mathbb{R}^3\) with fixed magnitude, while its EKF error state is represented by a two-dimensional tangent perturbation \(\delta\mathbf{g}\in\mathbb{R}^2\) on the \(\mathbb{S}^2\) manifold. Thus, the extended state associated with gravity is given by \(\mathbf{x}_{\mathrm{ext}} \triangleq \delta\mathbf{g}\). Other slowly varying calibration or sensor parameters can be appended in the same way, provided that their process and measurement Jacobian blocks are defined.

The augmented prediction is separated into the WNOJ process and sensor-extension blocks as \[\begin{align} \check{\mathbf{X}}^{\mathrm{WNOJ}}_{i+1} &= f_{\mathrm{WNOJ}}\!\left(\hat{\mathbf{X}}^{\mathrm{WNOJ}}_i,\Delta t\right), \\ \check{\mathbf{x}}_{\mathrm{ext},i+1} &= f_{\mathrm{ext}}\!\left(\hat{\mathbf{x}}_{\mathrm{ext},i}\right). \end{align} \label{eq:aug95process}\tag{32}\] Here, \(f_{\mathrm{WNOJ}}\) is given by Eqs. 22 26 . For the gravity extension used here, the nominal gravity direction is modeled as constant and the tangent-space error transition is identity.

The corresponding tangent-space transition matrix is partitioned as \[\mathbf{F}^{+}_{i+1} = \begin{bmatrix} \mathbf{F}_{\mathrm{WNOJ},i+1} & \mathbf{0}\\ \mathbf{0}& \mathbf{F}_{\mathrm{ext},i+1} \end{bmatrix}, \qquad \mathbf{F}_{\mathrm{WNOJ},i+1}=\boldsymbol{\Phi}'_{i+1}. \label{eq:aug95F}\tag{33}\] For the gravity extension, \(\mathbf{F}_{\mathrm{ext},i+1}=\mathbf{I}_2\). The augmented covariance prediction is \[\check{\mathbf{P}}^{+}_{i+1} = \mathbf{F}^{+}_{i+1}\hat{\mathbf{P}}^{+}_{i}(\mathbf{F}^{+}_{i+1})^\top + \mathbf{Q}^{+}_{i}, \label{eq:aug95cov95predict}\tag{34}\] with \[\mathbf{Q}^{+}_{i} = \mathop{\mathrm{diag}}\!\left( \boldsymbol{\Psi}'_{i+1}\mathbf{Q}_{t_i}(\boldsymbol{\Psi}'_{i+1})^\top,\, \mathbf{Q}_{\mathrm{ext},i} \right). \label{eq:aug95Q}\tag{35}\] Thus, the WNOJ block retains the covariance design derived in Section 4, while the extension block is added only when required by the sensor configuration. If a future extension state is coupled to the WNOJ motion in prediction, the off-diagonal blocks in Eq. 33 can be filled by the corresponding process Jacobians.

5.2 IMU Measurement Model (High-Frequency Update)↩︎

Unlike conventional LiDAR-inertial methods that integrate IMU in the prediction step, we treat IMU as a measurement update in the EKF. This enables the WNOJ prior to act as a high-order smoother for the raw IMU measurements, producing denoised estimates of acceleration and angular velocity.

An IMU measures the specific force \(\mathbf{a}_m\) and angular velocity \(\boldsymbol{\omega}_m\) in the body frame according to \[\mathbf{y}_{\text{IMU}} = \begin{bmatrix} \mathbf{a}_m \\ \boldsymbol{\omega}_m \end{bmatrix} = \begin{bmatrix} \mathbf{R}^\top(\mathbf{a} - \mathbf{g}) \\ \boldsymbol{\omega} \end{bmatrix} + \mathbf{n}_{\text{IMU}}, \label{eq:measurement95imu}\tag{36}\] where \(\mathbf{g}\) is the gravity vector in the world frame, estimated as an extended state in the real-data implementation, and \(\mathbf{n}_{\text{IMU}} \sim \mathcal{N}(\mathbf{0}, \boldsymbol{\Sigma}_{\text{IMU}})\) is the measurement noise. The measurement function is \(\mathbf{h}_{\text{IMU}}(\mathbf{X}^{+}) = [\mathbf{R}^\top(\mathbf{a} - \mathbf{g});\; \boldsymbol{\omega}]\).

We do not append accelerometer and gyroscope biases to the state in this paper. In the racing-car data, the effective bias is entangled with temperature variation and vibration-induced components, and preliminary simulations showed that directly estimating such biases in this WNOJ update formulation was unreliable. Because WNOJ-LIO directly estimates acceleration and angular velocity and constrains them through IMU measurements, residual IMU bias is handled through the measurement residual and covariance.

The IMU Jacobian is partitioned according to the augmented state, \(\mathbf{H}_{\text{IMU}}=[\mathbf{H}_{\text{IMU,WNOJ}}\;\mathbf{H}_{\text{IMU,ext}}]\). The WNOJ-state block contains non-zero columns for acceleration \(\mathbf{a}\), rotation \(\mathbf{R}\) (via \(\boldsymbol{\phi}\)), and angular velocity \(\boldsymbol{\omega}\). For the accelerometer part, the Jacobian with respect to \(\boldsymbol{\phi}\) involves \(\left[\mathbf{R}^\top(\mathbf{a}-\mathbf{g})\right]^\wedge\,\mathbf{J}_r(\boldsymbol{\phi})\) through the chain rule on \(\mathrm{SO}(3)\), and with respect to \(\mathbf{a}\) is simply \(\mathbf{R}^\top\). For the gyroscope part, the Jacobian with respect to \(\boldsymbol{\omega}\) is \(\mathbf{I}\), and all other WNOJ-state blocks are zero, giving \[{!}{\displaystyle \mathbf{H}_{\text{IMU,WNOJ}} = \begin{bmatrix} \mathbf{0}_{3} & \mathbf{0}_{3} & \mathbf{R}^\top& \left[\mathbf{R}^\top(\mathbf{a}-\mathbf{g})\right]^\wedge\,\mathbf{J}_r & \mathbf{0}_{3} & \mathbf{0}_{3} \\ \mathbf{0}_{3} & \mathbf{0}_{3} & \mathbf{0}_{3} & \mathbf{0}_{3} & \mathbf{I}_{3} & \mathbf{0}_{3} \end{bmatrix}}. \label{eq:H95imu}\tag{37}\] If gravity is included in \(\mathbf{x}_{\mathrm{ext}}\), let \(\mathbf{B}_{g}\in\mathbb{R}^{3\times2}\) denote the orthonormal tangent basis at the current gravity vector on \(\mathbb{S}^2\). The implementation uses the local retraction \(\mathbf{g}\boxplus\delta\mathbf{g}=\exp(\mathbf{B}_g\delta\mathbf{g})\mathbf{g}\), whose first-order Jacobian is \(\partial\mathbf{g}/\partial\delta\mathbf{g}=-\left[\mathbf{g}\right]^\wedge\mathbf{B}_{g}\). Therefore, the IMU extension block is \[\mathbf{H}_{\text{IMU,ext}} = \begin{bmatrix} \mathbf{R}^\top\left[\mathbf{g}\right]^\wedge\mathbf{B}_{g} \\ \mathbf{0}_{3\times2} \end{bmatrix}. \label{eq:H95imu95ext}\tag{38}\] This follows from \(\partial\mathbf{R}^\top(\mathbf{a}-\mathbf{g})/\partial\mathbf{g}=-\mathbf{R}^\top\) and the \(\mathbb{S}^2\) tangent-space gravity parametrization used in VINS-Mono [37]. The detailed \(\mathbb{S}^2\) basis construction is omitted because this gravity parametrization is an implementation detail rather than a contribution of this paper.

The IMU measurement update executes at 200 Hz (each IMU sample triggers a prediction + update cycle). After the EKF update, the posterior state estimates \(\hat{\mathbf{a}}\) and \(\hat{\boldsymbol{\omega}}\) are the denoised acceleration and angular velocity. This is fundamentally different from FAST-LIO-style approaches: rather than using raw \(\mathbf{a}_m\) and \(\boldsymbol{\omega}_m\) for forward propagation, the WNOJ-LIO refines these estimates through the high-order motion prior, filtering out high-frequency noise while preserving the true dynamic signal.

5.3 Point Cloud Motion Compensation Using Posterior State History↩︎

During a LiDAR scan interval \([t_{\ell,s},t_{\ell,e}]\), each point \(\mathbf{p}^{\mathrm{raw}}_{\ell,m}\) is acquired at its own timestamp \(\tau_{\ell,m}\in[t_{\ell,s},t_{\ell,e}]\) and therefore corresponds to a different sensor pose. To compensate for the resulting motion distortion, WNOJ-LIO utilizes a history of posterior poses generated by IMU-rate EKF updates, rather than relying on poses obtained through direct integration of raw IMU measurements.

Let the stored IMU-update times be \(\mathcal{T}_I=\{t_{I,i},t_{I,i+1},\ldots,t_{I,i+K}\}\). After each IMU update, the EKF stores the corresponding posterior pose \[\hat{\mathbf{T}}_{I,j} = \begin{bmatrix} \hat{\mathbf{R}}_{I,j} & \hat{\mathbf{p}}_{I,j} \\ \mathbf{0}& 1 \end{bmatrix}, \qquad j=i,\ldots,i+K, \label{eq:posterior95pose95history}\tag{39}\] where \(\hat{\mathbf{R}}_{I,j}\) and \(\hat{\mathbf{p}}_{I,j}\) denote the IMU-rate posterior orientation and position estimates, respectively. The arrival time of each LiDAR scan is taken as the reference time \(t_{\ell,\mathrm{ref}}\) for motion compensation, and the pose predicted at this time is denoted by \(\check{\mathbf{T}}_{\ell,\mathrm{ref}}\).

Since the adjacent posterior poses are separated by only a short time interval, a computationally efficient first-order interpolation scheme is adopted to compute the sensor pose at the LiDAR point acquisition time. For each LiDAR point timestamp \(\tau_{\ell,m}\), two neighboring IMU posterior poses \(\hat{\mathbf{T}}_{I,j}\) and \(\hat{\mathbf{T}}_{I,j+1}\) are selected from the stored history. The interpolation ratio is \[\beta_{\ell,m} = \frac{\tau_{\ell,m}-t_{I,j}}{t_{I,j+1}-t_{I,j}}. \label{eq:interp95beta}\tag{40}\] The LiDAR point-time pose is reconstructed through linear interpolation of translation and geodesic interpolation of rotation \[\begin{align} \hat{\mathbf{p}}_{\ell,m} &= (1-\beta_{\ell,m})\hat{\mathbf{p}}_{I,j} + \beta_{\ell,m}\hat{\mathbf{p}}_{I,j+1}, \\ \hat{\mathbf{R}}_{\ell,m} &= \hat{\mathbf{R}}_{I,j} \exp\!\left(\beta_{\ell,m} \log(\hat{\mathbf{R}}_{I,j}^\top\hat{\mathbf{R}}_{I,j+1})\right), \\ \hat{\mathbf{T}}_{\ell,m} &= \begin{bmatrix} \hat{\mathbf{R}}_{\ell,m} & \hat{\mathbf{p}}_{\ell,m} \\ \mathbf{0}& 1 \end{bmatrix}. \end{align} \label{eq:T95interpolate}\tag{41}\] Although the interpolation is performed locally between two adjacent posterior poses, the interpolated trajectory inherits the temporal smoothness imposed by the WNOJ prior as well as the corrections introduced by the IMU measurement updates.

Each raw point is subsequently transformed to the LiDAR reference frame associated with \(\hat{\mathbf{T}}_{\ell,\mathrm{ref}}\) \[\mathbf{p}^{\text{corr}}_{\ell,m} = \check{\mathbf{T}}_{\ell,\mathrm{ref}}^{-1}\,\hat{\mathbf{T}}_{\ell,m}\,\mathbf{p}^{\text{raw}}_{\ell,m}. \label{eq:de95distortion}\tag{42}\] Unlike FAST-LIO, which performs motion compensation using poses obtained through direct integration of raw IMU measurements, the proposed approach relies on a sequence of posterior poses generated by repeated IMU update steps. Consequently, high-frequency IMU noise is attenuated before entering the motion-compensation process, reducing the propagation of IMU-induced errors to the LiDAR measurements used in the subsequent point-to-plane EKF update.

5.4 LiDAR Measurement Model (Low-Frequency Update)↩︎

After de-distortion, the LiDAR scan is aligned with the local map through point-to-plane registration, with map maintenance and local plane fitting performed following the FAST-LIO mapping strategy. For each selected de-distorted point \(m\in\mathcal{S}_{\ell}\), the implementation transforms \(\mathbf{p}^{\text{corr}}_{\ell,m}\) to the world frame and fits a local plane from its nearest map points. Let this plane be \(\mathbf{n}_{m}^{\top}\mathbf{x}+d_m=0\), where \(\mathbf{n}_{m}\) is the world-frame unit normal and \(d_m\) is the plane offset. The LiDAR measurement function and residual are \[\begin{align} h_{\text{LiDAR},m}(\mathbf{X}^{+}) &= \mathbf{n}_{m}^{\top}\!\left(\mathbf{R}\mathbf{p}^{\text{corr}}_{\ell,m}+\mathbf{p}\right)+d_m, \qquad m\in\mathcal{S}_{\ell}, \end{align} \label{eq:measurement95lidar}\tag{43}\] where the zero point-to-plane distance is treated as the LiDAR measurement. The corresponding WNOJ-state measurement-function Jacobian used in the EKF update is \[{!}{\displaystyle \mathbf{H}_{\text{LiDAR,WNOJ},m} = \left[ \mathbf{n}_{m}^{\top},\; \mathbf{0}^\top,\; \mathbf{0}^\top,\; -\mathbf{n}_{m}^{\top}\mathbf{R}\left[\mathbf{p}^{\text{corr}}_{\ell,m}\right]^\wedge\,\mathbf{J}_r(\boldsymbol{\phi}),\; \mathbf{0}^\top,\; \mathbf{0}^\top\right]}. \label{eq:H95lidar}\tag{44}\] For the gravity-augmented WNOJ-LIO state, the LiDAR residual does not directly depend on the gravity extension, so \(\mathbf{H}_{\text{LiDAR,ext}}=\mathbf{0}\).

The LiDAR measurement update executes at 20 Hz (after each scan completion), correcting the position \(\mathbf{p}\) and attitude \(\mathbf{R}\) components of the state. Combined with the high-frequency IMU update, this dual-frequency architecture provides continuous state refinement.

5.5 Dual-Frequency EKF Update↩︎

The complete measurement update follows the standard EKF formulation on the augmented state. Given a measurement vector \(\mathbf{y}_{i+1}\) with measurement function \(\mathbf{h}(\mathbf{X}^{+}_{i+1})\) and noise covariance \(\boldsymbol{\Sigma}_{\mathbf{y}}\), the residual and Jacobian are defined as \[\begin{align} \mathbf{r}_{i+1} &= \mathbf{y}_{i+1} - \mathbf{h}(\check{\mathbf{X}}^{+}_{i+1}), \\ \mathbf{H}_{i+1} &= \begin{bmatrix} \mathbf{H}_{\mathrm{WNOJ},i+1} & \mathbf{H}_{\mathrm{ext},i+1} \end{bmatrix}. \end{align} \label{eq:H95partition}\tag{45}\] The Kalman update is \[\begin{align} \mathbf{K}_{i+1} &= \check{\mathbf{P}}^{+}_{i+1}\mathbf{H}_{i+1}^\top \left(\mathbf{H}_{i+1}\check{\mathbf{P}}^{+}_{i+1}\mathbf{H}_{i+1}^\top+ \boldsymbol{\Sigma}_{\mathbf{y}}\right)^{-1}, \\ \hat{\mathbf{P}}^{+}_{i+1} &= \left(\mathbf{I}- \mathbf{K}_{i+1}\mathbf{H}_{i+1}\right)\check{\mathbf{P}}^{+}_{i+1}, \\ \hat{\mathbf{X}}^{+}_{i+1} &= \check{\mathbf{X}}^{+}_{i+1} \boxplus^{+} \mathbf{K}_{i+1}\mathbf{r}_{i+1}, \end{align} \label{eq:ekf95update}\tag{46}\] where \(\boxplus^{+}\) applies the manifold perturbation to the WNOJ block and, for the gravity extension, applies the \(\mathbb{S}^2\) retraction to the two-dimensional tangent perturbation. For the IMU update, \(\mathbf{H}_{\mathrm{WNOJ}}=\mathbf{H}_{\text{IMU,WNOJ}}\) and \(\mathbf{H}_{\mathrm{ext}}=\mathbf{H}_{\text{IMU,ext}}\) (Eqs. 37 38 ). For the LiDAR update, \(\mathbf{H}_{\mathrm{WNOJ}}=\mathbf{H}_{\text{LiDAR,WNOJ}}\) and \(\mathbf{H}_{\mathrm{ext}}=\mathbf{0}\) (Eq. 44 ).

6 Simulation Study↩︎

This section evaluates WNOJ-LIO through a controlled simulation against a FAST-LIO-style baseline inspired by FAST-LIO [10], [34]. The baseline preserves the key FAST-LIO mechanism in which raw IMU measurements drive state prediction, while using the same simulated point-to-plane LiDAR measurements as WNOJ-LIO. This controlled setting removes mapping-related effects and isolates the influence of the inertial propagation strategy. The evaluation is organized around two inertial sensing conditions and one ablation: normal IMU noise, high IMU noise, and a WNOJ-LIO variant without IMU updates. This structure tests whether the proposed WNOJ prediction plus IMU-measurement-update design remains effective when raw inertial propagation becomes noisy, and separates the contribution of the WNOJ prior from that of the IMU measurements.

6.1 Simulation Setup↩︎

As shown in Fig. 3, a simulated environment of size \(40\times50\times10\) m is constructed. To evaluate the influence of different IMU integration strategies on odometry estimation independently of mapping quality, 20 known planar patches are distributed on the surrounding walls and floor instead of using a dynamically built map. The centroids of these patches are used to generate point-to-plane LiDAR measurements, thereby decoupling odometry estimation from mapping errors.

A continuous-time six-degree-of-freedom ground-truth trajectory and the corresponding 200 Hz IMU measurements are generated using a public simulation framework⁴. To assess robustness across inertial sensing conditions, two IMU measurement sets are synthesized from the same continuous-time acceleration and angular-velocity ground truth. The normal-noise setting uses zero-mean white noise with standard deviations \(\sigma_a = 0.0294\) m/s\(^2\) and \(\sigma_{\omega} = 0.00175\) rad/s, with accelerometer and gyroscope bias random-walk coefficients of \(5\times10^{-4}\) (m/s\(^2\))/\(\sqrt{\text{s}}\) and \(5\times10^{-5}\) (rad/s)/\(\sqrt{\text{s}}\), respectively. The high-noise setting scales the IMU noise level to represent vibration-amplified MEMS measurements on high-dynamic platforms, where sensor output noise can increase substantially with mechanical excitation [13], [38], [39].

LiDAR measurements are simulated at 20 Hz, yielding a scan duration of 50 ms. During each scan, the 20 planar patch centroids are observed sequentially at uniformly spaced timestamps within the scan interval, yielding one LiDAR point per patch. The coordinates of these points are generated by evaluating the continuous-time trajectory at their respective acquisition times. Consequently, the resulting point cloud naturally exhibits intra-scan motion distortion. Independent Gaussian noise is further added to each LiDAR point measurement.

All methods use the same trajectory, planar patches, LiDAR timestamps, and noise realizations within each noise setting.

6.2 Acceleration and Angular-Velocity Denoising Evaluation↩︎

We first examine the effect of IMU noise on the estimated vehicle acceleration and angular velocity, which are used later for point cloud de-distortion. In this simulation, ground-truth vehicle acceleration and angular velocity are available, so the evaluation is not a direct comparison of IMU sensor outputs. In the FAST-LIO-style baseline, raw IMU acceleration and angular velocity are directly forward-integrated for state prediction, so IMU sensor noise propagates into the predicted trajectory and is subsequently transferred to LiDAR motion compensation. In contrast, WNOJ-LIO uses the IMU as a measurement update: the third-order WNOJ prior provides a smooth state prediction, and the noisy IMU measurement is fused through the Kalman gain to produce denoised posterior acceleration and angular-velocity estimates.

Figs. 4 and 5 compare the estimated vehicle acceleration and angular velocity under normal and large IMU noise, respectively. In both settings, the FAST-LIO-style prediction closely tracks the noisy IMU-driven propagation, whereas the WNOJ-LIO posterior produces smoother acceleration and angular-velocity estimates closer to the ground truth. The difference becomes more visible under large IMU noise, where direct IMU propagation transfers high-frequency noise into the estimated acceleration and angular velocity.

Table 2 quantifies the acceleration and angular-velocity RMSE under both IMU noise settings. Under normal IMU noise, WNOJ-LIO reduces acceleration RMSE by \(43.1\%\) and angular-velocity RMSE by \(79.8\%\) compared with the FAST-LIO-style propagation. Under high IMU noise, the reductions increase to \(69.1\%\) and \(93.3\%\), respectively. These results show that treating the IMU as an EKF measurement is particularly beneficial when raw inertial propagation becomes noisy.

4pt

6.3 Point Cloud De-distortion Evaluation↩︎

The denoised high-rate posterior state history directly affects LiDAR point cloud de-distortion quality. During each 50 ms scan period, both methods transform raw body-frame LiDAR points to the scan-end body frame using their respective state estimates. The FAST-LIO-style baseline uses forward-integrated IMU states, while WNOJ-LIO uses the high-frequency posterior pose history obtained after WNOJ prediction and IMU updates.

Table 3 reports the point-cloud RMSE after expressing each scan in the scan-end body frame. Without de-distortion, the raw point-cloud error is \(0.2653\) m. Both methods reduce this error, but WNOJ-LIO remains lower under both noise settings, especially under high IMU noise.

4pt

6.4 Localization and Ablation↩︎

The improved scan de-distortion leads to more accurate point-to-plane registration and consequently improves overall state estimation. Fig. 6 compares the evolution of position, velocity, and attitude errors for five configurations: FAST-LIO-style and WNOJ-LIO under both normal and high IMU noise, together with a WNOJ-LIO-NoIMU ablation. Under normal IMU noise, both methods achieve accurate localization, while WNOJ-LIO consistently yields lower position and attitude errors with comparable velocity accuracy. As the IMU noise increases, the performance gap becomes significantly larger. Because the FAST-LIO-style baseline directly integrates raw inertial measurements during prediction, inertial disturbances are propagated into both the predicted trajectory and the subsequent scan de-distortion, leading to pronounced error peaks. In contrast, WNOJ-LIO employs the WNOJ prior as the prediction model and incorporates IMU measurements only through recursive measurement updates, making the estimated trajectory substantially less sensitive to inertial noise.

Table 4 quantitatively confirms these observations. Under normal IMU noise, WNOJ-LIO reduces the position RMSE from \(0.0257\) m to \(0.0176\) m and the attitude RMSE from \(0.1098^\circ\) to \(0.0802^\circ\), while maintaining comparable velocity accuracy. Under high IMU noise, the advantage becomes much more pronounced, reducing the position RMSE from \(0.1193\) m to \(0.0247\) m, the velocity RMSE from \(0.2421\) m/s to \(0.1205\) m/s, and the attitude RMSE from \(0.3357^\circ\) to \(0.1227^\circ\). These results indicate that decoupling trajectory prediction from raw inertial integration substantially improves robustness against vibration-contaminated IMU measurements.

The WNOJ-LIO-NoIMU ablation further illustrates the complementary roles of the motion prior and inertial measurements. Although the WNOJ prior alone maintains relatively small position and attitude errors by enforcing a smooth physically consistent trajectory, the velocity estimate gradually drifts, resulting in a velocity RMSE of \(0.5132\) m/s. This behavior indicates that the motion prior provides strong geometric regularization but cannot fully recover dynamic states without external observations. High-frequency IMU updates therefore remain essential for accurately estimating velocity and other rapidly varying motion quantities.

4pt

7 Experimental Results↩︎

7.1 Racing-Car Evaluation Protocol↩︎

We evaluated WNOJ-LIO on four high-dynamic racing-car segments collected at Yas Marina Circuit, Abu Dhabi. This dataset is not a public benchmark. It is used here as a challenging real-world test with synchronized LiDAR, raw VectorNav IMU, VectorNav INS/GNSS, Kistler optical velocity, and vibration-isolated Bosch IMU measurements. The selected sequences span a wide range of operating conditions, from low-speed to extreme-speed driving, with maximum vehicle speeds ranging from 53 to 208 km/h. FAST-LIO was adopted as the baseline because it represents the standard LiDAR-inertial design in which the IMU is integrated in the prediction step. The proposed algorithm was implemented in C++ and released at https://github.com/LvJohny/wnoj-ekf-lio.git. All experiments reported in this paper were evaluated offline on collected data using a desktop computer equipped with an Intel Core i5-11600K CPU.

The reference sensors have different roles and reliability properties. The VectorNav INS/GNSS position is used as a position reference only when the RTK solution is fixed and converged. Position RMSE is therefore computed only at RTK-fixed epochs that overlap the LiDAR odometry timestamps within 0.1 s. Velocity is evaluated in the vehicle body frame against the Kistler optical speed sensor, using the forward and lateral velocity components. Attitude is evaluated against the VectorNav INS attitude after a static initial alignment between the local LiDAR-inertial frame and the ENU frame. The vibration-isolated Bosch IMU is not used by either LiDAR-inertial estimator; it is used only for the inertial-signal evaluation. This protocol avoids treating any single sensor as universal ground truth across all metrics.

7.2 Inertial Signal Evaluation↩︎

Before evaluating pose, velocity, and attitude, we directly examined the inertial signals used by the two LiDAR-inertial pipelines. The racing car carried a high-noise VectorNav IMU stream, denoted as the raw IMU input, and a vibration-isolated Bosch IMU, which was used only as an inertial-signal reference for this comparison. The Bosch measurements were calibrated into the VectorNav/vehicle body frame. Both FAST-LIO and WNOJ-LIO receive the same raw IMU input. FAST-LIO directly integrates this signal in prediction. For WNOJ-LIO, the compared angular velocity is the posterior body-frame angular velocity, while the compared acceleration signal is reconstructed by combining the estimated vehicle acceleration with the estimated gravity term and transforming it into the body frame. This comparison evaluates whether the WNOJ prior and high-rate IMU measurement update suppress vibration-induced inertial noise in the signal domain.

3pt

Table 5 quantifies the trend shown in Figs. 8 and 9. Across the four selected segments, WNOJ-LIO reduced total acceleration RMSE from 5.08-6.44 m/s\(^2\) to 0.71-1.47 m/s\(^2\). It also reduced total angular velocity RMSE from 0.024-0.047 rad/s to 0.011-0.038 rad/s. The largest improvement appears in acceleration, where the raw VectorNav signal contains strong vibration-induced components. The angular-velocity results show a smaller but consistent total-RMSE reduction across all segments, although individual axes can trade off under severe high-speed motion.

7.3 Position, Velocity, and Attitude Accuracy↩︎

Figs. 10-12 show representative localization results on the low-speed loop26_loc segment and the extreme-speed loop21_loc segment. The Kistler velocity plots show that both methods track the dominant forward-speed profile, while WNOJ-LIO is closer to the lateral-velocity reference in the high-speed segment. The attitude plots show larger roll and pitch deviations for FAST-LIO under the repeated high-speed maneuvers in loop21_loc. The position plots are shown only at RTK-fixed epochs and should therefore be interpreted as intermittent reference checks rather than continuous ground truth.

3pt

Table 6 summarizes all four racing-car segments. WNOJ-LIO achieved lower RTK-fixed position RMSE on loop26_loc, loop20_loc, and loop21_loc, while FAST-LIO was slightly lower only on loop38_loc. This position result should still be interpreted with the intermittent RTK-fixed GNSS reference and the vertical sensitivity seen in Fig. 12. In contrast, WNOJ-LIO achieved lower Kistler-referenced body-frame velocity RMSE and lower INS-referenced attitude RMSE on all four segments. The loop21_loc segment also shows the highest WNOJ-LIO runtime, with an average processing time of 78.63 ms/frame. These metrics are available over broader intervals than the RTK-fixed position samples and therefore provide complementary evidence for high-dynamic vehicle motion estimation.

7.4 Map Quality Observations↩︎

The generated point-cloud maps provide a qualitative check on the scan-to-map consistency that is not fully captured by intermittent RTK samples. Fig. 13 compares WNOJ-LIO and FAST-LIO on the extreme-speed loop21_loc segment. Both maps recover the main track geometry, but the FAST-LIO map shows thicker point dispersion and visible ghosting around several structures. The WNOJ-LIO map is more compact in these regions. This observation is used only as qualitative map evidence; the quantitative conclusions remain those reported in Table 6.

8 Conclusion↩︎

This paper presented WNOJ-LIO, a LiDAR-IMU fusion framework for high-dynamic motion estimation. Its dual-frequency architecture uses the IMU as a high-frequency EKF measurement update and LiDAR as a low-frequency geometric update for pose correction. The decoupled WNOJ process model evolves the global state on \(\mathbb{R}^3 \times \mathrm{SO}(3)\) and represents attitude perturbations in \(\mathfrak{so}(3)\), enabling recursive covariance propagation through the global-state transition Jacobian \(\boldsymbol{\Phi}'\), the local-to-global process-noise mapping \(\boldsymbol{\Psi}'\), and the closed-form integrated WNOJ covariance \(\mathbf{Q}_{t_i}\). This formulation transfers the WNOJ prior from batch trajectory estimation to a real-time recursive estimator. Simulations demonstrated improved acceleration and angular-velocity denoising, point-cloud de-distortion, and localization accuracy compared with a FAST-LIO-style baseline. Racing-car experiments on four segments spanning 53 to 208 km/h further yielded lower RMSE than FAST-LIO in total acceleration, angular velocity, body-frame velocity, and attitude across all four segments. Under the intermittent RTK-fixed reference, WNOJ-LIO maintained competitive position accuracy, yielding lower RMSE in three of the four segments. The qualitative map comparison on the extreme-speed segment also showed reduced point dispersion for WNOJ-LIO.

Future work will focus on adaptive WNOJ noise tuning, online reference-quality assessment, sliding-window extensions for LiDAR latency, and integration with visual measurements for LiDAR-visual-inertial odometry.

Acknowledgment↩︎

The authors would like to thank the data-collection team for their support during the racing-car tests.

References↩︎

---

1. Junning Lyu, Qizhi Guo, Xia Ning and Tao Song are with the School of Aerospace Engineering, Beijing Institute of Technology, Beijing 100081, China.↩︎

2. Shaoming He (corresponding author) is with the School of Aerospace Engineering, Beijing Institute of Technology, Beijing 100081, China. (e-mail: heshaoming@bit.edu.cn)↩︎

3. This work was supported by the National Key Research and Development Program Grant No. 2022YFE0204400.↩︎

4. https://github.com/robosu12/imu_data_simulation.git↩︎