Multi-sensor state estimation requires accurate spatial and temporal calibration between heterogeneous sensors. Existing spatiotemporal calibration methods typically represent sensor extrinsic parameters as a rigid-body transformation together with a scalar temporal offset, causing the spatial and temporal parameters to enter the estimation problem through separate mechanisms. While the spatial transformation composes directly with the robot pose, accounting for the temporal offset requires evaluating the trajectory at a shifted time, typically through motion compensation, interpolation, or a continuous-time trajectory representation. In this work, we formulate joint trajectory estimation and spatiotemporal calibration using the special Galilean group for the robot trajectory and the Aristotelian subgroup for sensor extrinsics. This representation incorporates the coupling between motion and temporal offset directly through the group structure, avoiding the need for a separately parameterized continuous-time trajectory. We develop twist- and IMU-driven motion priors, accommodate both ideal and non-ideal clocks, and evaluate the framework with camera, position, and range measurements. Simulated and real experiments demonstrate calibration and trajectory-estimation performance comparable to established methods across heterogeneous sensor configurations. Our simplified framework therefore offers a quality-of-life improvement over existing methods without compromising performance.
Figures & tables
Fig. 1: Trajectory estimation and calibration using the special Galilean group. (a) The robot trajectory is represented by states \bGs∈\SGal , expressed with respect to the inertial reference frame FI . The robot body frame and the inertial measurement unit (IMU) F\ri are assumed to be co-located. The spatial and temporal transformations to the camera F\rc are represented by an element of the Aristotelian group \bA\rc∈\Arist . (b) The IMU and camera sample asynchronously with periods ΔI and Δc . The camera measurements have a temporal offset \timeoffset\rc relative to the IMU/reference clock and may additionally experience a delay δc before reaching the estimator. As a result, the measurement timestamps observed by the estimator can differ from physical measurement times. The sensor extrinsic parameters, represented by the transformation \bA\rc , needs to be estimated along with the robot trajectory for reliable state estimation.
Fig. 2: Schematic of the IMU–camera extrinsic calibration setup. The Aristotelian element A\rc represents the spatial and temporal transformation from the camera frame F\rc to the IMU frame F\ri . A feature point r\rclj on the calibration target is observed by the camera, while FB and FI denote the target and inertial frames, respectively.
Fig. 3: Factor graph for batch trajectory estimation and calibration. The robot state, \bGj , and the inter-sensor extrinsics, \bA\re , are nodes representing the states to be estimated. We consider two types of factors: a motion prior generated using an interoceptive sensor, represented by solid black circles, and exteroceptive measurement factors, represented by solid blue circles.
Fig. 4: Ground-truth and estimated trajectories from simulation with a non-ideal IMU clock. Corrective measurements are provided by a position sensor, which also serves as the reference clock.
Fig. 5: Trajectory estimation error and residual clock error from simulations with a drifting IMU clock. Since the IMU clock drifts over time, using the non-ideal-clock model results in lower position and clock errors than using the ideal-clock model.
Fig. 6: (Top) Clock-error estimation results from simulation showing the estimated (Est.) clock error t^−s alongside the true clock error t−s . (Bottom) Residual clock error in the estimated reference time t^−t .
Fig. 7: Estimated ( b^# ) and true ( b# ) gyroscope (top) and accelerometer (bottom) biases from simulation with non-ideal IMU clock.
Fig. 8: Simulated trajectory for calibration of the extrinsic parameters between a twist sensor and a camera sensor. Landmark color indicates the number of observations across multiple frames during calibration.
Fig. 9: Trajectory estimation results from simulations involving a twist sensor paired with (i) a position sensor (top) and (ii) a camera (bottom), for different inter-sensor temporal offsets. Results are shown for three sensor-noise regimes, ranging from low noise to high noise. Values are from N=10 trials for each temporal offset value and noise regime.
Sensor noise ( 1σ )
Sensor
unit
low
medium
high
Twist ( ω )
rads−1−1
2.5e-4
2.5e-3
0.025
Twist ( v )
ms−1−1
1.25e-4
1.25e-3
0.0125
Camera
px
5e-3
5e-2
0.5
Position
m
1e-4
1e-3
0.01
TABLE I: Sensor-noise regimes for twist-based calibration. 2 2 footnotetext: Process-noise density of the motion prior ( 8 ), not a sensor parameter; listed here for reproducibility.
Sensor
Noise
ε\br\re[\mathrm{cm}]
ε\btheta\re[deg]
Position
low
0.071±0.054
—
medium
0.204±0.127
—
high
15.358±16.366
—
Camera
low
0.127±0.108
0.0756±0.0845
medium
0.152±0.109
0.0778±0.0690
high
2.991±2.191
0.8155±0.6638
TABLE II: Spatial extrinsic calibration errors from simulation with Twist-based motion model for different sensor-noise regimes, averaged over all temporal offsets and Monte Carlo trials ( N=30 trials per sensor). Values report mean ± one standard deviation of the error norm.
Fig. 10: Temporal offset estimation results from simulations under different sensor-noise regimes for a position sensor (top) and a camera (bottom). Errors are computed as the difference between the true value \timeoffset\re and the estimated value \timeoffset^\re from N=10 trials for each temporal offset value. Results for the high-noise regime are shown on the right.
Fig. 11: Results from simulations ( N=10 trials) for temporal offset estimation with a position sensor in the high-noise regime. The magnitude of the twist inputs to the motion model is increased to improve the estimation accuracy under higher sensor noise.
Fig. 12: Trajectory estimation results using an IMU paired with a camera, a position sensor, and a range sensor, for varying temporal offsets. Values are computed from N=10 trials for each temporal-offset and sensor modality.
Sensor
ε\br\re[\mathrm{cm}]
ε\btheta\re[\mathrm{\SIUnitSymbolDegree}]
Camera
0.158±0.114
0.0208±0.0129
Position
0.376±0.263
—
Range
1.101±0.451
—
TABLE III: Spatial extrinsic calibration errors from simulation with IMU-based motion model, averaged over all time offsets and Monte Carlo trials ( N=60 per sensor). Values are mean ± one standard deviation of the error norm.
Fig. 13: Temporal offset estimation results from simulations with IMU as the interoceptive sensor. Errors are calculated as the difference between true value \timeoffset\re and estimated value τ^\re from N=10 trials for each temporal offset and sensor modality.
Fig. 14: Estimated (twist–stereo cam.) and reference (mocap) trajectories with the 3D landmarks, from the Starry Night dataset.
Position RMSE [\mathrm{cm}]
Rotation RMSE [\mathrm{\SIUnitSymbolDegree}]
ε\timeoffset\rc[\mathrm{ms}]
2.72±0.08
2.97±0.12
0.00±0.14
TABLE IV: Estimation errors with artificial temporal offsets of 0 – 20\mathrm{ms}$$ injected into the stereo camera stream, reported as mean ± one standard deviation over the tested offsets.
\br\rc[\mathrm{cm}]
\bC\rc (angles) [\mathrm{\SIUnitSymbolDegree}]
\timeoffset\rc[\mathrm{ms}]
Kalibr
−1.725
−6.825
0.431
0.172
1.303
89.123
0.028
Ours
−1.725
−6.796
0.418
0.170
1.301
89.130
0.021
Deviation
ε\br\rc=0.032
ε\btheta\rc=0.007
ε\timeoffset\rc=0.007
Mean reprojection error [px] : Kalibr =0.426 , Ours =0.453
TABLE V: Camera–IMU sensor extrinsic calibration results on the EuRoC imu_april sequence in the monocular configuration with cam0 and imu0 .
ε\br\rc[\mathrm{cm}]
ε\btheta\rc[\mathrm{\SIUnitSymbolDegree}]
ε\timeoffset\rc[\mathrm{ms}]
0.026±0.006
0.022±0.004
0.0257±0.001
TABLE VI: Distribution of sensor extrinsic calibration errors with artificial temporal offsets ranging from −4\mathrm{ms} to $4\,$\mathrm{ms} on the imu_april sequence of EuRoC dataset.
Mean reproj. [px]
Seq.
ε\br\rc [ cm ]
ε\btheta\rc [ \SIUnitSymbolDegree ]
ε\timeoffset\rc [ ms ]
Kalibr
Ours
1
0.017
0.0154
0.002
0.137
0.134
2
0.029
0.0144
0.01
0.135
0.131
3
0.032
0.0141
0.021
0.134
0.116
4
0.025
0.0146
0.016
0.133
0.116
TABLE VII: Camera–IMU sensor extrinsic calibration results on dataset-calib-imu#_1024_16 sequences from the TUM-VI dataset. ε\br\rc , ε\btheta\rc and ε\timeoffset\rc are defined as in Table V .
\br\rc[\mathrm{cm}]
\bC\rc (angles) [\mathrm{\SIUnitSymbolDegree}]
\timeoffset\rc[\mathrm{ms}]
Kalibr
4.531
−7.303
−4.500
−88.028
0.496
178.325
0.114
Ours
4.530
−7.319
−4.506
−88.015
0.495
178.317
0.112
TABLE VIII: Estimated extrinsic parameters on dataset-calib-imu1_1024_16 sequence of the TUM-VI calibration dataset.
Fig. 15: (Top) Estimated t^−s and true clock t−s error using a non-ideal clock model on the EuRoC dataset. (Bottom) Residual clock error in the estimated reference time t^−t .
Ideal clock
Non-ideal clock
Mean reprojection error [px]
0.588
0.453
TABLE IX: Mean reprojection error of different clock models on the imu_april sequence with artificial drift applied to the IMU clock.
Fig. 16: Our test platform for data collection and calibration of sensor extrinsic parameters.
Fig. 17: Trajectories estimated with IMU–position (motion capture) and IMU–range (UWB) sensor combinations, alongside the reference trajectory from the motion-capture system (mocap).
Fig. 18: Estimated temporal offset between Vicon position measurements and IMU from real experiments using the baseline 3D motion correlation analysis (MCA) method and our method (Ours). Results for MCA are reported as mean ± one standard deviation.
Fig. 19: (Left) UWB distance measurements from a real experiment showing the unprocessed ranges to different anchors (A#) over time. (Right) The corresponding histogram of range-measurement residuals across all anchors after compensating for spatial biases and removing outliers.
Fig. 20: (Top) Estimated IMU–UWB temporal offset from five real experiments. (Bottom) Corresponding trajectory-estimation RMSE computed with ( w.\timeoffset^\rp ) and without ( w/o.\timeoffset^\rp ) compensating for the IMU–motion-capture temporal offset \timeoffset^\rp estimated in the previous experiment.
Position RMSE [\mathrm{cm}]
Rotation RMSE [\mathrm{\SIUnitSymbolDegree}]
ε\timeoffset\rd[\mathrm{ms}]
10.5±0.000
2.886±0.002
0.085±0.648
TABLE X: Estimation errors from a real experiment with artificial temporal offsets ranging from −30\mathrm{ms} to $30\,$\mathrm{ms} injected into the UWB range data. Values are reported as mean ± one standard deviation across the tested temporal offsets.
In this paper, we present a robotics-tailored exposition of Galilean symmetry that leverages the community's familiarity with and understanding of rigid-body transformations and pose representations. Our approach contrasts with common treatments in the physics literature that introduce Galilean symmetry as a stepping stone to Einstein's relativity. We show that the classical homogeneous matrix algebra known well to roboticist translates directly into the Galilean modeling framework and provides a powerful tool for tackling robotics problems involving inertial measurements and spatio-temporal variables. We demonstrate the potential of the approach by developing a powerful model for studying motion of vehicles in an Earth fixed rotating reference frame, and developing a fusion algorithm for measurements with uncertain time-stamps.
Robert Mahony, Jonathan Kelly, Stephan Weiss
Systems Theory and Robotics Group, Australian National University, Australia · Space and Terrestrial Autonomous Robotics Systems Laboratory, University of Toronto Canada. · Control of Networked Systems group, University of Klagenfurt, Austria.
The special Galilean group, usually denoted SGal(3), is a 10-dimensional Lie group whose important subgroups include the special orthogonal group, the special Euclidean group, and the group of extended poses. We briefly describe SGal(3) and its Lie algebra and show how the group structure supports a unified representation of uncertainty in space and time. Our aim is to highlight the potential usefulness of this group for several robotics problems.
Jonathan Kelly
Space & Terrestrial Autonomous Robotic Systems (STARS) Laboratory, University of Toronto Institute for Aerospace Studies (UTIAS), Toronto, Canada
In field robotics, acquiring independent large-scale reference trajectories more accurate than the evaluated estimates remains an open challenge. The domain is widely reliant on Absolute Trajectory Error (ATE) and Relative Pose Error (RPE), computed with automated tools, that rest on assumptions and evaluation parameters rarely made explicit. When unreported, the errors can be misleading and hinder fair comparisons. This paper introduces a trajectory-evaluation protocol for standardized and reliable accuracy assessment in state estimation, localization, and Simultaneous Localization And Mapping (SLAM). The approach combines a novel temporal alignment method based on curvature signals with an error metric normalized by travelled distance. We explicitly account for temporal synchronization, sampling alignment, and extrinsic calibration, quantifying their influence through a sensitivity analysis. The proposed protocol contributes to more rigorous, reproducible, and standardized trajectory evaluation.
Effie Daum, Daniele De Martini, Claire Dune +1
Northern Robotics Laboratory, Université Laval, Québec City, Québec, Canada · Mobile Robotics Group, Oxford Robotics Institute, University of Oxford, Oxford, United Kingdom