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

cs.ROOct 27, 2026

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

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

Organizations: Department of Electrical Engineering, Linköping University, 58183 Linköping, Sweden · Robotic Systems Lab (RSL), ETH Zürich, 8092 Zurich, Switzerland · Department of Mechanical and Industrial Engineering, University of Toronto, Toronto, ON M5S 3G8, Canada · Department of Biomedical Engineering, City University of Hong Kong, Hong Kong

Abstract

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.

Figures & tables

Fig. 1: RESPLE2 enables multi-sensor odometry across diverse scenarios on ANYmal D (a, b) and Unitree Go2 (e) legged robots, a tracked vehicle (c), a hybrid legged quadcopter (d), and handheld platforms (f, g, h). The Boxi sensor payload [ 10 ] was deployed for the ANYmal D and tracked vehicle.

Fig. 2: System overview of the proposed RESPLE2 framework.

Dataset	Deployment Scenario	Adopted Sensors ∗
GrandTour [ 42 ]	ANYmal D quadruped, indoor, outdoor, and wilderness environments	Hesai ( L ), Alpha ( V ), ICM40609 ( I ), leg joints ( K )
JEPLO [ 49 ]	Unitree Go2 quadruped, maneuvers over stairs and boxes	Mid360 ( L ), ICM40609 ( I ), leg joints ( K )
PPS TV (experiment)	Tracked vehicle, high-speed maneuvers in outdoor environments	Hesai ( L ), HDR ( V ), ICM40609 ( I )
HOPPER (experiment)	Legged quadcopter [ 22 ] , highly dynamic maneuvers in enclosed and open spaces	Mid360 ( L ), ICM40609 ( I1 ), BMI088 ( I2 )
HANDI (experiment)	Handheld platform, long-distance walking in indoor and outdoor campus environments	Mid360 ( L1 ), ICM40609 ( I1 ), Avia ( L2 ), BMI088 ( I2 ), D455 ( V1 ), ZedX1 ( V2 )

TABLE I: Public and experimental datasets for benchmarking.

		ARC-2 ∗		ARC-7 ∗		CON-4 ∗		EIG-1 ∗		SNOW-2 ∗		SPX-2 ∗		ARC-3 †		HIL-1 †		LEE-1 †		PIL-2 †
Method	Variant	APE	RPE	APE	RPE	APE	RPE	APE	RPE	APE	RPE	APE	RPE	APE	RPE	APE	RPE	APE	RPE	APE	RPE
CMLO	LO	$3.090$	$0.360$	$0.182$	$0.061$	$0.024$	$\underline{0.015}$	$0.041$	$0.023$	$0.023$	$0.016$	$0.104$	$0.030$	✗	✗	$0.063$	$0.025$	✗	✗	✗	✗
CoLIC	LVIO	$0.095$	$0.028$	$0.052$	$\bf{0.005}$	$0.027$	$\bf{0.005}$	$0.048$	$\bf{0.006}$	$\bf{0.012}$	$\bf{0.005}$	$0.040$	$\bf{0.006}$	$0.154$	$\bf{0.019}$	$0.031$	$0.009$	$0.051$	$\underline{0.006}$	${0.031}$	$0.013$
FLIVO2	LVIO	$\bf{0.012}$	$0.009$	$\bf{0.013}$	$\underline{0.014}$	$0.030$	$\underline{0.015}$	$\underline{0.033}$	$0.014$	$0.016$	$0.016$	$\underline{0.023}$	$0.013$	✗	✗	$\bf{0.018}$	$\underline{0.008}$	$0.037$	$0.011$	$0.071$	$0.025$
RESPLE	LIO	$0.109$	$0.055$	$0.347$	$0.142$	$0.054$	$0.024$	$0.107$	$0.022$	$0.026$	$0.021$	$0.042$	$\underline{0.012}$	✗	✗	$0.036$	$0.009$	$0.041$	$0.020$	✗	✗
	LIO	$\bf{0.012}$	$\bf{0.006}$	$0.036$	$0.016$	$\underline{0.016}$	$0.021$	$0.034$	$0.008$	$0.014$	$\underline{0.009}$	$\bf{0.022}$	$0.013$	$0.078$	$0.024$	$0.022$	$\bf{0.004}$	$\underline{0.020}$	$\bf{0.005}$	$0.029$	$0.010$

TABLE II: APE and RPE (RMSE, meters) on GrandTour .

Panel of Figure 5 — Fig. 3: Examples of RESPLE2 tested on JEPLO .

Method	CoLIC	FLIVO2	RESPLE	LeKILO	RESPLE2
Variant	LIO	LIO	LIO	LKIO	LIO	LKIO
EIL_Box	$0.054$	$3.200$	${0.011}$	$0.032$	$\underline{0.006}$	$\bf{0.005}$
EIL_Mask1	$0.032$	$0.551$	$0.015$	$0.032$	$\underline{0.009}$	$\bf{0.008}$
EIL_Mask2	$0.045$	$0.830$	${0.015}$	$0.040$	$\underline{0.011}$	$\bf{0.010}$
EIL_Mix	$0.028$	$1.628$	$\underline{0.015}$	$0.028$	$\bf{0.010}$	$\bf{0.010}$
EIL_Stairs	$0.047$	✗	$0.013$	$0.052$	$\underline{0.010}$	$\bf{0.007}$

TABLE III: APE (RMSE, meters) on JEPLO .

		CMLO	CoLIC		FLIVO2		RESPLE		RESPLE2
		LO	LIO	LVIO	LIO	LVIO	LO	LIO	LIO	LVIO
DO	APE	$0.082$	$0.051$	$0.051$	$0.582$	${0.046}$	✗	$0.066$	$\underline{0.042}$	$\bf{0.041}$
DO	RPE	$0.079$	$\underline{0.007}$	$\underline{0.007}$	$0.044$	$\underline{0.007}$	✗	$0.018$	$\bf{0.006}$	$\underline{0.007}$
	APE	$0.063$	${0.026}$	${0.026}$	${0.026}$	$0.028$	$0.030$	$\underline{0.024}$	$\underline{0.024}$	$\bf{0.020}$
DH	RPE	$0.060$	$\underline{0.006}$	$\underline{0.006}$	$0.009$	$0.009$	$0.020$	$0.012$	$\bf{0.005}$	$\bf{0.005}$
NH	APE	$0.071$	$0.029$	$0.029$	${0.024}$	$\underline{0.023}$	$0.034$	$0.025$	$0.025$	$\bf{0.022}$

TABLE IV: APE and RPE (RMSE, meters) on PPS Tracked Vehicle .

Panel of Figure 8 — Fig. 4: (a) Tracked vehicle equipped with Boxi for collecting the PPS TV dataset. (b) RESPLE2 results on the day_street sequence.

Panel of Figure 9 — Fig. 5: Legged quadcopter (a) used to collect the HOPPER dataset, with examples of flip (b) and lateral jump (c) from flip4 .

Fig. 6: Acceleration along the $z$ -axis during high2 (left) and angular velocity norms during flip2 (right). Red and black curves correspond to the external and internal IMUs, respectively.

Panel of Figure 11 — Fig. 7: RESPLE2 tested on HOPPER . Black and red curves represent the estimated and ground-truth trajectories, respectively.

Method	PLIO		CoLIC	FLIVO2	RESPLE		RESPLE2
Variant	ext	int	ext	ext	ext	int	ext	int	LO
base1	$0.022$	$0.178$	$0.008$	$\underline{0.007}$	$0.010$	$0.016$	$\bf{0.005}$	$0.014$	$0.017$
high1	$0.022$	$0.556$	$0.020$	$0.019$	$\underline{0.011}$	$0.019$	$\bf{0.005}$	$0.017$	$0.021$
high2	$0.022$	$0.594$	$0.017$	$0.021$	$\underline{0.011}$	$0.023$	$\bf{0.005}$	$0.020$	$0.024$
side1	$0.019$	$0.178$	$\underline{0.008}$	$\underline{0.008}$	$\underline{0.008}$	$0.018$	$\bf{0.004}$	$0.016$	$0.017$
side2	$0.019$	$0.149$	$\underline{0.008}$	$\underline{0.008}$	$\underline{0.008}$	$0.017$	$\bf{0.004}$	$0.015$	$0.015$

TABLE V: APE (RMSE, meters) on HOPPER .

Method	PLIO		CoLIC	FLIVO2	RESPLE		RESPLE2
IMU	ext	int	ext	ext	ext	int	ext	int	LO
base1	$0.245$	$1.674$	$\bf{0.081}$	$0.254$	$0.732$	$0.798$	$\underline{0.111}$	$0.617$	$1.019$
high1	$0.294$	$2.098$	$\underline{0.123}$	$0.269$	$0.683$	$0.775$	$\bf{0.097}$	$0.678$	$1.079$
high2	$0.261$	$2.509$	$\underline{0.125}$	$0.353$	$0.734$	$0.924$	$\bf{0.105}$	$0.809$	$1.264$
side1	$0.256$	$1.836$	$\bf{0.077}$	$0.236$	$0.582$	$0.756$	$\underline{0.107}$	$0.591$	$0.993$
side2	$\bf{0.123}$	$1.757$	$0.218$	$0.290$	$0.705$	$0.802$	$\underline{0.210}$	$0.675$	$1.068$

TABLE VI: Velocity estimation error (RMSE,

\text{\,}\mathrm{m}

) on HOPPER .

Fig. 8: Position (top) and velocity (bottom) along the

z

-axis on flip1 .

Method	CoLIC	FLIVO2	RESPLE	RESPLE2
Variant	LVIO	LVIO	LIO	LIO	LVIO (+LC)
az-base1	$0.121$	$0.059$	$\underline{0.034}$	${0.036}$	$\bf{0.028}$ ( N/A )
az-base2	✗	✗	$0.085$	$\underline{0.036}$	$\bf{0.033}$ ( N/A )
az-dyna1	$0.105$	${0.080}$	$0.157$	$\bf{0.055}$	$\underline{0.059}$ ( N/A )
az-dyna2	$0.074$	$0.074$	$1.588$	$\bf{0.053}$	$\underline{0.054}$ ( N/A )
az-dyna3	$0.141$	$\underline{0.095}$	$4.489$	$15.944$	$\bf{0.059}$ ( N/A )

TABLE VII: APE (RMSE, meters) on HANDI .

Panel of Figure 16 — Fig. 9: RESPLE2 on az-dyna sequences. Black and red curves are estimated and ground-truth trajectories, respectively.

Panel of Figure 17 — Fig. 10: Comparison before and after global refinement on az-out2 .

Fig. 11: Position correction and preservation of body-frame angular (blue) and linear velocities (black) at the first global spline optimization.

Fig. 12: Ablation of exact RCP prediction on flip1 in

(x,y)

-coordinates w.r.t. ground truth (red). The IMU-predicted control points and those predicted using the constant transition matrix

{\mathbf{A}}

in Section IX-A1 are shown in blue and green with the corresponding B-spline estimates, respectively.

	base1	high1	high2	side1	side2	flip1	flip2	flip3	flip4
Pred. w/o IMU	$0.015$	$0.017$	$0.012$	$0.010$	$0.011$	$0.015$	$0.016$	$0.018$	$0.017$
Pred. w/ IMU	$\bf{0.005}$	$\bf{0.004}$	$\bf{0.005}$	$\bf{0.004}$	$\bf{0.004}$	$\bf{0.008}$	$\bf{0.008}$	$\bf{0.011}$	$\bf{0.009}$

TABLE VIII: Position APE (RMSE, meters) on HOPPER for ablation on inertial-informed prediction.

Panel of Figure 21 — Fig. 13: Comparison of knot placement and motion estimates between RESPLE2 and Coco-LIC on a segment of flip1 , with reference to ground truth (a) and IMU measurements (b). RESPLE2 achieves higher estimation accuracy and fidelity with a knot distribution that better adapts to motion.

Fig. 14: Runtime profile of global spline optimization on ar-inout , including per-iteration cost, total optimization time, and knot counts.

Sequence	flip1	day_open	Outdoor1
LiDAR preprocessing	$1.570$	$2.765$	$0.810$
Image preprocessing	–	$3.346$	–
Visual feature generation	–	$0.756$	–
Knot extension	$0.372$	$2.926$	$0.380$
RCP prediction	$0.363$	$0.184$	$0.107$
RCP reset	$0.142$	$0.163$	$0.090$

TABLE IX: Runtime (

\mathrm{m}\mathrm{s}

per LiDAR frame) analysis.

Robot State Estimation

Robot state estimation infers a robot’s position, motion, orientation, and internal configuration from noisy sensors and physical constraints.

See all 200 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
ASPIRE-VINS: Adaptive Spline-based Visual-inertial Navigation System With Robust 3D Measurement Residuals
Aug 13, 2026Kwangyik Jung, Eungchang Mason Lee, Taekjun Oh +1Trajectory Optimization B-Spline Trajectory Representation
Parallel Reference-Centric Continuous-Time Relative Localization with Augmented Clamped Non-Uniform B-Splines
Feb 25, 2026Jiadong Lu, Zhehan Li, Tao Han +4Relative Pose Estimation Multi-Robot Systems
State Key Laboratory of Industrial Control Technology, Institute of Cyber-Systems and Control, Zhejiang University, Hangzhou, 310027, China. · Huzhou Institute of Zhejiang University, Huzhou, 313000, China. · The School of Automation and Intellegent Sensing, Shanghai Jiao Tong University, Shanghai, 200240, China. +1

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

Robot State Estimation

Explore similar work

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

ASPIRE-VINS: Adaptive Spline-based Visual-inertial Navigation System With Robust 3D Measurement Residuals

Parallel Reference-Centric Continuous-Time Relative Localization with Augmented Clamped Non-Uniform B-Splines