[Paper Review] A White-Noise-On-Jerk Motion Prior for Continuous-Time Trajectory Estimation on SE(3)

Tim Y. Tang, David J. Yoon+1 more|arXiv (Cornell University)|Sep 17, 2018

Time Series Analysis and Forecasting4 citations

TL;DR

This paper proposes a white-noise-on-jerk (WNOJ) motion prior for continuous-time trajectory estimation on SE(3), improving upon the standard white-noise-on-acceleration (WNOA) prior in Gaussian process-based simultaneous trajectory estimation and mapping (STEAM). By modeling the prior mean to encourage constant acceleration, the new formulation reduces bias and improves accuracy, especially in trajectories with non-zero acceleration, achieving up to 9% error reduction on real-world lidar datasets compared to WNOA.

ABSTRACT

Simultaneous trajectory estimation and mapping (STEAM) offers an efficient approach to continuous-time trajectory estimation, by representing the trajectory as a Gaussian process (GP). Previous formulations of the STEAM framework use a GP prior that assumes white-noise-on-acceleration, with the prior mean encouraging constant body-centric velocity. We show that such a prior cannot sufficiently represent trajectory sections with non-zero acceleration, resulting in a bias to the posterior estimates. This paper derives a novel motion prior that assumes white-noise-on-jerk, where the prior mean encourages constant body-centric acceleration. With the new prior, we formulate a variation of STEAM that estimates the pose, body-centric velocity, and body-centric acceleration. By evaluating across several datasets, we show that the new prior greatly outperforms the white-noise-on-acceleration prior in terms of solution accuracy.

Motivation & Objective

• To address bias in continuous-time trajectory estimation when the motion prior cannot represent non-zero acceleration.
• To improve the accuracy of Gaussian process-based trajectory estimation in scenarios with dynamic motion, such as urban driving.
• To derive and validate a novel motion prior that assumes white noise on jerk, enabling better representation of acceleration changes.
• To demonstrate that the new prior outperforms the standard white-noise-on-acceleration prior in real-world lidar datasets with motion distortion.
• To enable more accurate continuous-time pose and velocity estimation by including acceleration in the state vector.

Proposed method

• Derives a new stochastic differential equation (SDE) for motion modeling, assuming white noise on jerk, leading to a WNOJ prior.
• Extends the STEAM framework to include body-centric acceleration in the state vector, enabling joint estimation of pose, velocity, and acceleration.
• Uses a continuous-time Gaussian process prior with a kernel derived from the WNOJ SDE, ensuring smooth and physically plausible trajectory priors.
• Solves the estimation problem via batch optimization, maintaining sparsity in the inverse kernel matrix for computational efficiency.
• Tunes the process noise covariance matrix $\mathbf{Q}_c$ using the KITTI training set and applies it across all datasets for fair comparison.
• Performs point cloud registration using interpolated poses derived from the continuous-time GP posterior, compensating for motion distortion in scanning lidar data.

Experimental results

Research questions

• RQ1 Can a white-noise-on-jerk motion prior reduce bias in continuous-time trajectory estimation compared to the standard white-noise-on-acceleration prior?
• RQ2 Does modeling constant acceleration in the prior lead to improved accuracy in real-world lidar datasets with non-uniform motion?
• RQ3 How does the inclusion of acceleration in the state vector affect estimation performance and computational cost?
• RQ4 To what extent does the WNOJ prior improve trajectory estimation in motion-distorted lidar data compared to WNOA?
• RQ5 Can the new prior generalize across diverse driving scenarios, including urban, suburban, and highway environments?

Key findings

• The WNOJ prior reduced overall trajectory error by 9% on the University of Toronto dataset, which features frequent acceleration changes in urban driving.
• On the Richmond Hill dataset, the new estimator achieved an 8.4% reduction in overall error compared to the baseline WNOA estimator.
• The WNOJ prior significantly improved trajectory accuracy in sequences with high dynamic motion, such as turning and decelerating at intersections.
• The new estimator demonstrated reduced bias in odometry estimates, particularly in motion-distorted point clouds, as shown in 3D trajectory plots.
• Despite a 50% increase in optimization cost due to the inclusion of acceleration in the state, the overall runtime increase was modest due to invariant costs in other components.
• The WNOJ prior was more sensitive to the process noise covariance $\mathbf{Q}_c$, but tuning on the KITTI dataset alone yielded consistent improvements across all test datasets.