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

TL;DR

This work addresses continuous-time trajectory estimation for states on Lie groups by deriving a global Gaussian process prior using the Magnus expansion, thereby avoiding the common patchwork of local GPs. The method discretizes the linearized Lie-group SDE in a principled way, yielding a global state transition and discrete process-noise covariance that respect the manifold geometry. Implemented as a factor-graph-based estimator with a querying mechanism, the approach is benchmarked against a strong local GP baseline, showing comparable accuracy and interpolation quality but with higher computational cost. The results suggest that the Magnus-based global GP prior offers a geometry-consistent alternative with potential benefits for certain motion models and high-rate sensing, motivating future work on speedups and broader Lie-group applications.

Abstract

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.

Figures (12)

• Figure 1: In continuous-time estimation, we consider that a sensor is moving smoothly through space over time. At discrete times, the sensor takes measurements $\mathbf{y}_k$ of the environment (e.g., images, lidar scans, etc.) that can be used to estimate the trajectory. The trajectory itself is represented as a continuous-time function, $\mathbf{x}(t)$, often using a GP prior, regularizing the trajectory to be smooth and physically plausible.
• Figure 2: High-level comparison of two different ways to define a Gaussian process prior for continuous-time trajectory estimation: (left) the baseline approach using local LTI SDE stitched together, and (right) the proposed approach using a global SDE.
• Figure 3: Commutative diagram showing the various pathways from the desired continuous-time Gaussian process prior motion model in the top left to the discrete-time linearized error in the bottom right. The key to making this diagram self-consistent is the use of the Magnus expansion. Symbols explained in the text.
• Figure 4: Factor graph representation of the GP prior on $SE(3)$. The large circular nodes represent the states to be estimated, while the small black circular nodes represent factors that encode probabilistic constraints between the states.
• Figure 5: Factor graph representation of querying the trajectory at an arbitrary time between two measurement times. We imagine that there was a state, $\{\mathbf{T}_\tau, {\boldsymbol{\varpi}}_\tau\}$, or ${\boldsymbol{\varepsilon}}_\tau$ in the perturbation variables, in the original factor graph at the query time. This state was eliminated to obtain a new binary factor (the motion prior between $k-1$ and $k$) and a conditional density for ${\boldsymbol{\varepsilon}}_\tau$. We can solve for the states at $k-1$ and $k$ in the main solve, then use the conditional density to obtain the mean and covariance of the queried state. Moreover, we do not need to know the time of the query ahead of time; we can perform this operation after the main solve is complete and as many times as desired.

Theorems & Definitions (2)

• Theorem B.1
• proof