A Third-Order Gaussian Process Trajectory Representation Framework with Closed-Form Kinematics for Continuous-Time Motion Estimation
Authors: Thien-Minh Nguyen, Ziyu Cao, Kailai Li, William Talbot, Tongxing Jin, Shenghai Yuan, Timothy D. Barfoot, Lihua Xie
Organizations: School of Electrical and Electronic Engineering, Nanyang Technological University, 50 Nanyang Avenue, Singapore 639798. · Department of Electrical Engineering, Linköping University, 58183 Linköping, Sweden. · Bernoulli Institute for Mathematics, Computer Science and Artificial Intelligence, University of Groningen, 9747 AG Groningen, the Netherlands. · Robotic Systems Lab (RSL), ETH Zürich, 8092 Zürich, Switzerland. · Autonomous Space Robotics Laboratory (ASRL), University of Toronto, Canada.
In this paper, we propose a third-order, i.e., white-noise-on-jerk, Gaussian Process (GP) Trajectory Representation (TR) framework for continuous-time (CT) motion estimation (ME) tasks. Our framework features a unified trajectory representation that encapsulates the kinematic models of both SO(3)×R3 and SE(3) pose representations. This encapsulation strategy allows users to use the same implementation of measurement-based factors for either choice of pose representation, which facilitates experimentation and comparison to achieve the best model for the ME task. In addition, unique to our framework, we derive the kinematic models with the closed-form temporal derivatives of the local variable of SO(3) and SE(3), which so far has only been approximated based on the Taylor expansion in the literature. Our experiments show that these kinematic models can improve the estimation accuracy in high-speed scenarios. All analytical Jacobians of the interpolated states with respect to the support states of the trajectory representation, as well as the motion prior factors, are also provided for accelerated Gauss-Newton (GN) optimization. Our experiments demonstrate the efficacy and efficiency of the framework in various motion estimation tasks such as localization, calibration, and odometry, facilitating fast prototyping for ME researchers. We release the source code for the benefit of the community. Our project is available at https://github.com/brytsknguyen/gptr.
Figures & tables
Fig. 1 : Top: DT trajectory representation. Due to a finite number of represented states, we need to approximate, perform extra processing, or use sophisticated hardware synchronization to restrict the coupling of the observations with the state estimates. Middle vs Bottom: Comparison of B-spline and GP-based representations of continuous time trajectory. Under the B-spline scheme, the poses and temporal derivatives can be inferred from the control points, which do not lie on the trajectory. By contrast, GP support states include trajectory poses with explicit temporal derivatives according to a chosen prior motion model.
TABLE I : Derivations of the kinematic models for GP on SO(3) manifold
Fig. 2 : The dependency graphs describing the hierarchy of manifold variables for SO(3) GP (Fig. 2(a) ) and SE(3) GP.
Fig. 3 : The factor graph of the GP-based multi-LiDAR coupled-motion estimation scheme. We have two trajectories corresponding to two LiDARs. Each square represents one factor, and the lines indicate the coupled states. The extrinsic prior factors reinforce the a priori .
Fig. 4 : Illustration of the estimated trajectory (blue) and ground truth (red) in the visual-inertial calibration given known landmark positions depicted as green dots.
Fig. 5 : The RMSE of VI experiments with 0.1s (top) and 0.2s (bottoms) MPIs under various time factors. In the legends, SO(3)×R3 and SE(3) refer to the chosen pose representations of trajectory estimate, while AP and CF respectively refer to our implementation by GPTR with the approximated kinematics model and the closed-form model. A STEAM-based implementation would be equivalent to the SE(3) +AP settings. We can see that at low speed, the CF and AP models have similar accuracy. However, at higher speed or sparser Δt , the AP model starts gaining higher error compared to the CF model. This divergence of performance between CF and AP can be viewed more easily for the SE(3) (top left and bottom left) than the SO(3)×R3 .
Fig. 6 : The ratio of solve time between the CF and AP models corresponding to the VI experiments in Fig. 5 . Notice that there are several experiments where the CF model has much lower solve time. This is because the Ceres solver finds that the problem has converged after fewer iterations.
Fig. 7 : RMSE of the range-only optimization experiments with different values of frequency parameter Ω . In the legends, SO(3)×R3 and SE(3) refer to the pose representation used, while AP and CF refer to the approximated and closed-form kinematic models. For the split ground truth trajectory in ( 53 ), the SE(3) +X models (left plots) start having an RMSE above 0.2m quickly with larger Ω , while the SO(3)×R3 +X representation (right plots) maintains an RMSE below 0.2m. On the contrary, in Fig. 7(b) , the SO(3)×R3 pose representation (left plots) starts losing track of the true trajectory quickly when Ω increases, while the SE(3) representation maintains smaller RMSE in all experiments. This demonstrates the importance of choosing the appropriate pose representation.
Fig. 8 : Ground truth trajectories for experiments with UWB-only experiments in Sec. VI-B .
Fig. 9 : The ratio of solving time by closed-form kinematics model and the AP model in the ranging-based optimization problem, using SO(3)×R3 (left) and SE(3) (right) representations.
Fig. 10 : RMSE of the lidar-based batch optimization experiments.
Fig. 11 : The two-lidar sensor suite. Based on the illustration in the Mid360 Datasheet, we assume the origin of the lidar’s coordinate system coincides with the center of the half-spheres.
Fig. 12 : Trajectory estimate for each lidar by the MLCME scheme vs the ground truth.
Fig. 13 : Left: the prior map. Middle: the lidar point clouds, undistorted and overlaid on the prior map. Right: the distorted raw point cloud over the prior map.
Seq#
Lidar
I 2 EKF
Traj
CTEM
GPTR
SE(3)
SO(3)×R3
0
0
0.0476
0.027
0.043
0.0236
0.0233
1
0.6793
0.0442
0.0417
0.0285
0.0272
1
0
0.0647
0.0278
0.042
0.0234
0.0233
1
0.043
0.1319
0.0418
0.0316
0.031
2
0
0.0477
0.0422
0.0554
0.0401
0.0394
TABLE II : Comparing the RMSE of trajectory estimate by the MLCME scheme in GPTR and other methods with the handheld setup data.
Fig. 14 : The extrinsic estimate result between LiDAR 0 and LiDAR 1.
Fig. 15 : Simulated setup with two LiDARs mounted on top of a ground robot. The ground robot moves in a room with six flat sides. The two LiDARs have no overlap. The GPTR trajectory and I2EKF-LO estimates are marked by green and blue, respectively, while the ground truth trajectories are in red.
Fig. 16 : The estimation error of the extrinsic parameters between LiDAR 0 and LiDAR 1.
Ω
Lidar
I 2 EKF
Traj
CTEM
GPTR
SE(3)
SO(3)×R3
0.25
0
0.0571
0.0635
0.0363
0.0344
0.0343
1
0.0495
0.0585
0.0397
0.0392
0.0392
0.35
0
0.0576
0.0633
0.0359
0.0343
0.0341
1
0.0506
0.0586
0.0419
0.0405
0.0405
0.45
0
0.058
0.0632
0.0378
0.0337
0.0332
TABLE III : Comparing the RMSE of trajectory estimate by GPTR’s MLCME scheme and other methods with the synthetic data at different values of Ω .
Continuous-time state estimation is gaining in popularity due to its abilities to provide smooth solutions, handle asynchronous sensors, and interpolate between data points. While there are two main paradigms, parametric (e.g., temporal basis functions, splines) and nonparametric (Gaussian processes), the latter has seen less adoption despite its technical advantages and relative ease of implementation. In this article, we seek to rectify this situation by providing a new simplified explanation of GP continuous-time estimation rooted in the language of factor graphs, which have become the de facto estimation paradigm in much of robotics. To simplify onboarding, we also provide three working examples implemented in the popular GTSAM estimation framework.
Connor Holmes, Sven Lilge, Zi Cong Guo +2
University of Toronto · Georgia Institute of Technology
Tracking non-stationary covariance matrices is fundamental to vision yet hindered by existing estimators that either neglect manifold constraints or rely on first-order updates, incurring inevitable phase lag during rapid evolution. We propose K-GMRF, an online, training-free framework for covariance tracking that reformulates the problem as forced rigid-body motion on Lie groups. Derived from the Euler-Poincaré equations, our method interprets observations as torques driving a latent angular velocity, propagated via a structure-preserving symplectic integrator. We theoretically prove that this second-order dynamics achieves zero steady-state error under constant rotation, strictly superior to the proportional lag of first-order baselines. Validation across three domains demonstrates robust tracking fidelity: (i) on synthetic ellipses, K-GMRF reduces angular error by 30x compared to Riemannian EMA while maintaining stability at high speeds; (ii) on SO(3) stabilization with 20% dropout, it decreases geodesic error from 29.4° to 9.9°; and (iii) on OTB motion-blur sequences, it improves loU from 0.55 to 0.74 on BlurCar2 with a 96% success rate. As a fully differentiable symplectic module, K-GMRF provides a plug-and-play geometric prior for data-constrained scenarios and an interpretable layer within modern deep architectures.
ZhiMing Li
School of Computer Science and Technology, Tianjin University
Continuous-time SLAM provides a principled framework for fusing heterogeneous sensors while estimating smooth trajectories, and is particularly well-suited for handling heterogeneous, asynchronous sensor streams with non-uniform readout patterns, such as rolling shutter cameras, LiDAR scanners, radar sweeps, or event-based sensors. In this work, we introduce G-solver, a fully Gaussian and distributed framework that combines Gaussian Belief Propagation (GBP) with Gaussian Process (GP) motion priors for continuous-time trajectory estimation. Our GP model provides a probabilistic representation of the trajectory, enabling consistent interpolation and the use of data-driven hyperparameters, while GBP offers a scalable message-passing formulation well-suited for decentralized settings. The resulting solver naturally extends to multi-camera scenarios without specialized synchronization or engineering effort. We evaluate the approach on synthetic and real data, including rolling shutter and distributed multi-camera optimization, demonstrating accurate and stable estimation with runtimes comparable to existing continuous-time methods. An open-source implementation is released.
Davide Ceriola, Simone Ferrari, Luca Di Giammarino +2
Department of Computer, Control, and Management Engineering “Antonio Ruberti”, Sapienza University of Rome, Italy · University of Stuttgart, Germany