Revisiting Continuous-Time Trajectory Estimation via Gaussian Processes and the Magnus Expansion

Continuous-time state estimation has been shown to be an effective means of (i) handling asynchronous and high-rate measurements, (ii) introducing smoothness to the estimate, (iii) post hoc querying the estimate at times other than those of the measurements, and (iv) addressing certain observability issues related to scanning-while-moving sensors. A popular means of representing the trajectory in continuous time is via a Gaussian process (GP) prior, with the prior's mean and covariance functions generated by a linear time-varying (LTV) stochastic differential equation (SDE) driven by white noise. When the state comprises elements of Lie groups, previous works have resorted to a patchwork of local GPs each with a linear time-invariant SDE kernel, which while effective in practice, lacks theoretical elegance. Here we revisit the full LTV GP approach to continuous-time trajectory estimation, deriving a global GP prior on Lie groups via the Magnus expansion, which offers a more elegant and general solution. We provide a numerical comparison between the two approaches and discuss their relative merits.

Continuous-time Trajectory Estimation: A Comparative Study Between Gaussian Process and Spline-based Approaches

Continuous-time trajectory estimation is an attractive alternative to discrete-time batch estimation due to the ability to incorporate high-frequency measurements from asynchronous sensors while keeping the number of optimization parameters bounded. Two types of continuous-time estimation have become prevalent in the literature: Gaussian process regression and spline-based estimation. In this paper, we present a direct comparison between these two methods. We first compare them using a simple linear system, and then compare them in a camera and IMU sensor fusion scenario on SE(3) in both simulation and hardware. Our results show that if the same measurements and motion model are used, the two methods achieve similar trajectory accuracy. In addition, if the spline order is chosen so that the degree-of-differentiability of the two trajectory representations match, then they achieve similar solve times as well.