A Data-Driven Motion Prior For Continuous-Time Trajectory Estimation On Se(3)

Simultaneous trajectory estimation and mapping (STEAM) is a method for continuous-time trajectory estimation in which the trajectory is represented as a Gaussian Process (GP). Previous formulations of STEAM used a GP prior that assumed either white-noise-on-acceleration (WNOA) or white-noise-on-jerk (WNOJ). However, previous work did not provide a principled way to choose the continuous-time motion prior or its parameters on a real robotic system. This letter derives a novel data-driven motion prior where ground truth trajectories of a moving robot are used to train a motion prior that better represents the robot's motion. In this approach, we use a prior where latent accelerations are represented as a GP with a Matern covariance function and draw a connection to the Singer acceleration model. We then formulate a variation of STEAM using this new prior. We train the WNOA, WNOJ, and our new latent-force prior and evaluate their performance in the context of both lidar localization and lidar odometry of a car driving along a 20 km route, where we show improved state estimates compared to the two previous formulations.