PaperlayerView exact revision on arXiv

Source-linked AI summary

RESPLE2: Inertial-Informed Recursive Non-Uniform Spline Estimation for Multi-Sensor Odometry with Global Refinement

Ziyu Cao, William Talbot, Song Li, Pakpong Chirarattananon, Fredrik Gustafsson, Marco Hutter, Kailai Li

arXiv:2610.37602v1cs.RO

TL;DR

Continuous-time multi-sensor odometry faces challenges from asynchronous measurements, motion distortion, and the deployment demands of sliding-window optimization. RESPLE2 uses an inertial-informed recursive non-uniform B-spline estimator with global refinement; evaluations report competitive or superior accuracy and robustness in real time.

  • Problem

    Asynchronous measurements and motion distortion challenge discrete-time odometry, while sliding-window optimization can complicate real-time deployment and sensor integration.

    Source: §I · pp. 1–2

  • Method

    RESPLE2 recursively estimates non-uniform B-spline control points using inertial-informed prediction and supports multi-sensor fusion and direct loop-closure refinement.

    Source: Contributions · p. 3, §X · pp. 15–16

  • Results

    Across diverse platforms and sensor configurations, RESPLE2 achieves comparable or superior odometry accuracy and robustness while maintaining real-time operation.

    Source: Contributions · p. 3, §X · p. 16

  • Takeaways & Limitations

    The framework supports flexible combinations of LiDAR, visual, inertial, and leg-kinematic measurements, as well as LiDAR-only operation.

    Source: §X · p. 16

Abstract

from arXiv · show

We present RESPLE2, a recursive multi-sensor state estimation framework using non-uniform B-splines for 6-DoF motion representation. By exploiting the knot-clamping boundary property, we propose a theoretically grounded mechanism for exact control-point prediction directly informed by inertial kinematics along the trajectory, with knot placement adapted to motion and measurement availability. Within an iterated extended Kalman filter, we perform this prediction using a subset of inertial measurements. A subsequent reset step preserves trajectory consistency as the knots extend. The update step further incorporates heterogeneous sensor measurements at their native timestamps through continuous-time observation models, supporting flexible combinations of LiDARs, cameras, inertial measurement units, and leg kinematics. We further develop a lightweight global optimization module that directly refines non-uniform B-spline control points by incorporating loop-closure constraints derived from LiDAR-visual cues. We conduct extensive evaluations on public and experimental datasets across handheld, tracked, legged, and hybrid aerial-legged platforms, covering highly dynamic maneuvers with extreme accelerations. Compared with state-of-the-art multi-sensor odometry systems, RESPLE2 achieves competitive or superior estimation accuracy and robustness in the corresponding sensor configurations, particularly under aggressive motion, while maintaining real-time operation. We publicly release our code and experimental datasets at https://github.com/ASIG-X/RESPLE2.

I. INTRODUCTION

RESPLE2 addresses gaps in recursive continuous-time multi-sensor odometry with a non-uniform B-spline estimator and inertial-informed control-point prediction. It also supports direct global refinement and evaluation across varied platforms and sensor configurations.

I. INTRODUCTION

  • Discrete-time methods approximate asynchronous readings and may require motion-distortion compensation, while sliding-window continuous-time optimization can challenge real-time deployment and sensor integration.Recursive continuous-time estimators remain scarce and support a narrow set of configurations; non-uniform spline methods also lacked exact inertial prediction and native loop closure.Source: §I · pp. 1–2

Contributions

  • RESPLE2 combines recursive non-uniform B-spline estimation, exact inertial-informed control-point prediction, versatile sensor fusion, and direct global control-point refinement at loop closures.Evaluations span multiple platforms and sensor configurations, with comparable or superior accuracy and robustness in real time.Source: §I · p. 2, Contributions · p. 3

A. Multi-Sensor Odometry

  • Prior multi-sensor systems include tightly coupled filtering and optimization, while RESPLE2 builds on VoxelMap and feature-based visual residuals to prioritize robustness and versatile sensor designs.The cited design choices support LVIO, LKIO, and additional configurations.Source: A. Multi-Sensor Odometry · p. 3

B. Continuous-Time Motion Estimation

  • Continuous-time odometry includes GP-based methods and sliding-window B-spline optimization; RESPLE introduced recursive uniform-spline estimation, while RESPLE2 uses non-uniform knots for exact inertial-informed prediction and native global refinement.Prior knot-placement strategies use IMU excitation or visual keyframe distribution, and existing control-point initialization schemes did not exactly predict new points along the trajectory.Source: B. Continuous-Time Motion Estimation · pp. 3–4

A. Notation Conventions

  • The notation distinguishes scalars, vectors, matrices, continuous-time B-spline functions, quaternion operations, and coordinate frames.For example, R(·) maps a unit quaternion to its rotation matrix, and frame superscripts identify world, camera, or robot-base coordinates.Source: A. Notation Conventions · p. 4

B. Non-Uniform B-Spline Motion Representation

  • The motion representation uses non-uniform cubic B-splines, with position and quaternion-valued orientation trajectories determined by knots and control points.The non-uniform cubic basis depends on local knots; each segment uses four control points and is locally determined by six knots.Source: B. Non-Uniform B-Spline Motion… · p. 4

C. Kinematic Interpolations

The section derives continuous-time position and orientation kinematics from B-spline trajectories for use in multi-sensor IEKF estimation. It also describes recursive control-point estimation and the limitations of prior propagation models during rapid motion.

C. Kinematic Interpolations

  • B-spline position and orientation derivatives provide the continuous-time velocity, acceleration, and angular velocity used by the multi-sensor IEKF.Position derivatives follow from the spline basis; angular velocity is computed recursively rather than by directly differentiating the orientation spline.Source: C. Kinematic Interpolations · p. 4, 1) Positions · p. 4, 2) Orientations · p. 4

1) Positions:

  • Position kinematics are linear in the control points, so their Jacobians follow directly; orientation and angular-velocity Jacobians are obtained by the chain rule.The cited derivation for orientation Jacobians is in.Source: 1) Positions · p. 4, 2) Orientations · p. 4

D. Spline-State-Space Model for Recursive State Estimation

  • The spline-state-space model recursively estimates recurrent control points, with Ak propagating them and h mapping interpolated kinematics at sensor timestamps into the measurement domain.The measurement model accounts for measurement noise; the process model includes process noise.Source: D. Spline-State-Space Model for… · p. 5
  • Prior TriS extensions lacked non-uniform multi-sensor estimation, while propagation from preceding intervals without current kinematic information can yield inaccurate priors during rapid motion.The passage notes that weak observations in perceptually degenerate scenes may limit subsequent correction.Source: D. Spline-State-Space Model for… · p. 5

IV. SYSTEM OVERVIEW

RESPLE2 combines non-uniform B-spline motion estimation with inertial-informed recursive prediction and timestamped multi-sensor updates. Knot clamping supports exact endpoint control-point prediction, while reset and global refinement maintain and improve the trajectory as it extends.

IV. SYSTEM OVERVIEW

  • RESPLE2 buffers LiDAR, IMU, camera, and leg measurements and fuses them at their timestamps using continuous-time kinematic interpolation and modality-specific observation models.The IMU samples are divided into prediction and update subsets; LiDAR correspondences are formed against a local voxel map.Source: §IV · p. 5
  • After a knot is added, RESPLE2 unclamps the previous endpoint and resets control points and covariance over the finalized spacing before fusing buffered measurements.The global module also refines non-uniform spline control points from loop closures detected using iBTC.Source: §IV · p. 5

A. Generalized Spline-State-Space Model

  • The generalized spline-state-space model propagates recurrent control points using IMU readings assigned to prediction, with process and measurement noise represented in the model.The state includes position and orientation control-point components and accelerometer and gyroscope biases.Source: A. Generalized Spline-State-Space Model · pp. 5–6

B. Process Model

  • Clamping a new knot at the non-uniform spline boundary places its associated control point on the trajectory, enabling direct prediction from inertial measurements.For the cubic spline, the latest control point remains at the trajectory endpoint as the boundary advances.Source: B. Process Model · p. 6, 1) Knot-Clamping Boundary Property · p. 6

1) Knot-Clamping Boundary Property:

  • At the endpoint, the clamped cubic spline passes exactly through the new position and orientation control points, with endpoint velocities tied to their final increments.Position velocity and acceleration at the endpoint depend on the final control points and knot spacing.Source: 1) Knot-Clamping Boundary Property · p. 6

2) Exact Inertial-Informed RCP Propagation:

  • The new control point is progressively propagated across IMU substeps using strapdown kinematics, with acceleration compensated for gravity and bias and rotation accumulated from gyroscope readings.The process model is discretized between consecutive IMU measurements within the current knot interval.Source: 2) Exact Inertial-Informed RCP… · p. 6

C. Multi-Sensor Measurement Model

The measurement model instantiates a sensor-specific observation function for LiDAR and defines a point-to-plane residual using the interpolated body pose and local map. It accounts for uncertainty in both the measured point and map plane.

C. Multi-Sensor Measurement Model

  • The framework defines LiDAR observations as one sensor-specific instantiation of its multi-sensor observation function.The same framework introduces models for IMU, camera, and joint measurements.Source: C. Multi-Sensor Measurement Model · p. 6

1) LiDAR Point-to-Plane Metric:

  • The LiDAR metric transforms each timestamped raw point into the world frame using sensor extrinsics and the interpolated body pose, then measures its distance to an associated local-map plane.The plane association uses a nearest-neighbor point and normal from VoxelMap.Source: 1) LiDAR Point-to-Plane Metric · pp. 6–7
  • The point-to-plane residual uncertainty incorporates covariance for both the map plane and raw LiDAR point.The map plane carries covariance over its normal and center; point covariance is derived from range and bearing noise.Source: 1) LiDAR Point-to-Plane Metric · p. 7

2) IMU Metric:

The measurement models handle IMU, visual, and legged-kinematics observations using interpolated motion at each measurement timestamp. Their residuals include robust weighting or contact-based constraints where specified.

2) IMU Metric:

  • Accelerometer residuals use adaptive block-Huber weighting based on their Mahalanobis norm to suppress outliers from noise and potential saturation.A single scalar weight scales the precision of all three acceleration components.Source: 2) IMU Metric · p. 7

3) Visual Projection Metric:

  • Visual feature observations project LiDAR-depth-initialized 3D points through the interpolated camera pose, with Huber weighting used to suppress association outliers.Features are tracked between images using KLT optical flow.Source: 3) Visual Projection Metric · p. 7

4) Legged Kinematics Metric:

  • Legged measurements provide a world-frame foot-velocity metric and a foot-to-plane metric, with the latter assuming no slippage during contact.The foot velocity combines body motion, angular motion about the foot lever arm, and forward-kinematics foot velocity.Source: 4) Legged Kinematics Metric · p. 7

VI. INERTIAL-INFORMED ITERATED EXTENDED KALMAN FILTER ON NON-UNIFORM B-SPLINES

The filter recursively estimates non-uniform B-spline control points and IMU biases, initializing new control points from the clamped boundary and propagating them with an IMU-based process model. Knot extension uses LiDAR geometric sufficiency and motion-related criteria to balance available information and interval limits.

VI. INERTIAL-INFORMED ITERATED EXTENDED KALMAN FILTER ON NON-UNIFORM B-SPLINES

  • The EKF state comprises non-uniform B-spline control points and IMU biases, whose mean and covariance are recursively propagated through the process model.The process noise includes IMU measurement and bias noise.Source: §VI · p. 7, A. Inertial-Informed RCP Prediction · p. 8

A. Inertial-Informed RCP Prediction

  • At the previous clamped knot, the filter initializes the augmented state from the prior posterior and sets the new position and orientation control points to the previous knot's values.The new orientation increment is initialized to zero.Source: A. Inertial-Informed RCP Prediction · p. 7

Knot Extension:

  • A knot is introduced when LiDAR and IMU sample counts pass thresholds or velocity change exceeds its limit; the LiDAR batch must also pass both geometric sufficiency tests.The tests require a minimum eigenvalue threshold and a minimum relative contribution from the weakest translational direction.Source: Knot Extension · p. 8

B. RCP Reset for Unclamping

Adding a knot shifts the clamped boundary and changes the local spline basis, so RESPLE2 resets control points to preserve trajectory quantities at the previous endpoint. The filter then incorporates buffered multi-sensor measurements through an iterated update.

Unclamping:

  • After knot extension, the reset compensates for unclamping that could change the trajectory prior over the previous interval and affect data association.It restores position, velocity, acceleration, orientation, and angular velocity at the previous knot.Source: Unclamping · p. 8

Position Reset:

  • The position reset preserves position, velocity, and acceleration at the previous knot.The reset uses a closed-form linear transformation.Source: Position Reset · p. 8

Orientation Reset:

  • The orientation reset preserves orientation and angular velocity at the previous knot, solving the residual iteratively with Newton's method.Only the two preceding orientation increments need resetting for this residual.Source: Orientation Reset · pp. 8–9

C. Iterated Update

  • The iterated update refines the predicted state using buffered multi-modal measurements until the increment falls below a threshold or the iteration limit is reached.The resulting posterior includes an updated covariance.Source: C. Iterated Update · p. 9

D. Filter Setup

The filter combines inertial-informed control-point prediction, an RCP reset, and an iterated measurement update. Its sensor models include a legged kinematic measurement with a derived zero-velocity Jacobian.

D. Filter Setup

  • The recursive filter algorithm performs IMU-informed control-point prediction, resets the RCPs, and then returns the posterior after the iterated update.The prediction and reset also transform the state covariance.Source: D. Filter Setup · p. 9
  • The framework derives the legged zero-velocity measurement Jacobian with respect to position and orientation control points.The IMU-bias Jacobian blocks are zero, and the foot-position Jacobian resembles the LiDAR residual Jacobian.Source: D. Filter Setup · p. 9
  • For LiDAR, IMU, and camera measurement models, the update uses Jacobians that the authors describe as straightforward to derive.The legged model's foot-position observation also has a Jacobian resembling the LiDAR residual.Source: D. Filter Setup · p. 9

VII. GLOBAL REFINEMENT

RESPLE2 detects LiDAR-visual loop closures and uses them to refine the global non-uniform spline while constraining adjacent control points with odometry. The section also describes the implementation and benchmarking setup across public and experimental datasets.

A. Loop Closure Detection

  • iBTC retrieves historical keyframe candidates using geometric and image-assisted descriptors, verifies matches by geometric registration, and supplies accepted loop closures as global optimization constraints.The constraints pair timestamps with relative 6-DoF transformations.Source: A. Loop Closure Detection · pp. 9–10

B. Global Spline Optimization

  • Global refinement directly optimizes non-uniform spline control points using loop-closure and adjacent-control-point odometry constraints, preserving local kinematic consistency with sparse residual support.Each odometry residual involves only two control points, and the sparse problem is solved with Levenberg-Marquardt.Source: B. Global Spline Optimization · p. 10

A. Implementation

  • The ROS2 implementation separates multi-sensor odometry from global refinement and supports LiDAR-only, LiDAR-inertial, visual-inertial, and legged sensor configurations.The LiDAR-only variant replaces the inertial-informed process model with a constant-velocity model.Source: A. Implementation · p. 10

B. Benchmarking Setup

  • Benchmarking uses GrandTour, JEPLO, PPS TV, HOPPER, and HANDI datasets, compares against several state-of-the-art odometry systems, and reports ground-truth trajectory errors including fixed-increment RPEs.RPEs are computed over fixed 0.5 m path-length increments.Source: B. Benchmarking Setup · pp. 10–11

C. Public Datasets

Across GrandTour, JEPLO, PPS TV, and HOPPER, RESPLE2 is evaluated on diverse platforms and motion conditions, with competitive or lower reported errors across sensor configurations. The HOPPER tests include extreme maneuvers and reduced-quality inertial measurements.

1) GrandTour:

  • On GrandTour, RESPLE2 variants achieve competitive or superior accuracy overall, and LKVIO delivers the best overall performance among the RESPLE2 variants.RESPLE2-LIO has lower APE and RPE than RESPLE on nearly all sequences.Source: 1) GrandTour · p. 11

2) JEPLO:

  • RESPLE2 achieves the best overall APE on JEPLO, with LKIO attaining the lowest error on all eight sequences.Adding leg kinematics improves or maintains accuracy across all sequences.Source: Table III · p. 11, 2) JEPLO · p. 11

1) PPS TV:

  • On PPS TV, RESPLE2 has low APE and RPE in both LIO and LVIO; in LVIO, its APE is lower than Coco-LIC’s on every sequence.Adding visual measurements generally reduces RESPLE2 and FAST-LIVO2 errors, while it does not improve Coco-LIC’s reported accuracy.Source: Table IV · p. 12, 1) PPS TV · p. 12

2) HOPPER:

  • HOPPER tests RESPLE2 on flips, jumps, and flight, including accelerations around 25 g that exceed the built-in IMU’s ±4 g range.The dataset includes 12 indoor sequences and an external IMU with a ±24 g range.Source: 2) HOPPER · p. 12
  • On HOPPER, RESPLE2 has the lowest position APE on all sequences with both IMUs except flip4 with the internal IMU, and remains competitive against external-IMU methods.With the internal IMU, RESPLE2 also has higher velocity-estimation accuracy than all other systems on every sequence under the same configuration.Source: 2) HOPPER · pp. 12–13, Table V · p. 13, Table VI · p. 13

3) HANDI:

HANDI results show RESPLE2 completing all evaluated LIO and LVIO sequences, with particularly strong LVIO performance in degenerate and long-distance settings. The experiments also report benefits from inertial-informed prediction and motion-adaptive knot placement, while runtime remains within real-time budgets.

3) HANDI:

  • Global refinement further reduced long-distance LVIO errors, with position errors reduced by nearly 2 m at the first ar-inout loop closure while angular and linear velocities were preserved.Gains were especially pronounced in unstructured or geometrically degenerate environments such as az-corri and az-out2.Source: 3) HANDI · pp. 13–14
  • At least one RESPLE2 variant achieved the lowest odometry error on every evaluated HANDI sequence; LVIO led in degenerate corridors, indoor-outdoor transitions, and outdoor end-to-end error.HANDI comprises 13 sequences across varied handheld sensor configurations and environments.Source: 3) HANDI · p. 13, Table VII · p. 14

1) Inertial-Informed Prediction:

  • Inertial-informed control-point prediction reduced position APE on every HOPPER sequence by approximately 39%–76%, with the largest reduction on high1, which has extremely high accelerations.Under knot clamping, the newest control point coincides with the trajectory endpoint, providing an IMU-integrated endpoint prior for the measurement update.Source: 1) Inertial-Informed Prediction · p. 14

2) Knot Placement:

  • On flip1, RESPLE2 concentrated knots around landing impacts and used 2087 control points versus Coco-LIC’s 2362, an 11.6% reduction, while achieving higher estimation accuracy.Its acceleration and angular-velocity estimates also tracked measurements more closely near impact peaks.Source: 2) Knot Placement · pp. 14–15

B. Runtime and Computational Analysis

X. CONCLUSION

  • The conclusion identifies adaptive sensor weighting and visual feature constraints independent of LiDAR-derived depth as future directions for improving robustness under sensor degeneracy.The framework combines non-uniform B-spline estimation, motion-adaptive knot placement, and loop-closure refinement across LiDAR, visual, inertial, and leg-kinematic measurements.Source: §X · pp. 15–16

APPENDIX

The appendix derives boundary relations under knot clamping and describes resetting position control points as the spline extends.

A. Derivation of the Knot-Clamping Boundary Property

  • The derivation imposes the cubic B-spline knot-clamping condition at the new knot to obtain the boundary basis coefficients and establish the position relation in equation (10).At the clamped boundary, the normalized time is u = 1 and its derivatives are evaluated before substitution into the kinematic interpolation formula.Source: A. Derivation of the Knot-Clamping… · p. 16
  • At the clamped boundary, the orientation is evaluated from the cumulative basis functions, while the angular-velocity derivation uses vanishing derivatives of the clamped basis functions.The stated recursion yields zero intermediate angular velocities at the boundary.Source: A. Derivation of the Knot-Clamping… · p. 16

1) Position RCP Reset:

  • The position reset keeps the latest control point unchanged and obtains post-reset control points from a closed-form linear system, preserving the represented position and derivatives across the knot change.The pre-reset and post-reset evaluations refer to the same instant, represented at opposite endpoints of adjacent spline intervals.Source: 1) Position RCP Reset · p. 16

2) Orientation RCP Reset:

The orientation reset uses residuals for orientation and angular velocity, while the covariance reset propagates increments through the reset transformation.

2) Orientation RCP Reset:

  • The orientation residual uses the vector part of a relative quaternion, and the angular-velocity residual is the difference between post-reset and pre-reset angular velocities.These residuals are derived at the knot transition instant.Source: 2) Orientation RCP Reset · p. 17
  • Newton’s method iteratively solves the nonlinear least-squares residual condition for the post-reset orientation increments.The residual Jacobian is derived with respect to the post-reset and pre-reset increments.Source: 2) Orientation RCP Reset · p. 17

3) Covariance Reset:

  • The covariance reset uses the position-reset Jacobian and the linearized orientation reset map to transform the control-point increments.The unchanged increment is represented by an identity block; the remaining Jacobians are computed using the chain rule.Source: 3) Covariance Reset · p. 17

Related papers

Loading 2610.37602v1…