Batch Nonlinear Continuous-Time Trajectory Estimation as Exactly Sparse Gaussian Process Regression

TL;DR

This paper introduces a continuous-time Gaussian process regression framework for batch nonlinear trajectory estimation, leveraging sparse inverse kernel matrices for efficient computation and applicable to various vehicle dynamics models.

Contribution

It presents a novel approach that results in exactly sparse inverse kernel matrices for nonlinear continuous-time trajectory estimation, enabling efficient smoothing and interpolation.

Findings

Sparse inverse kernel matrices enable efficient computation.

Method is equivalent to classical smoothing in linear cases.

Effective in nonlinear trajectory estimation tasks.

Abstract

In this paper, we revisit batch state estimation through the lens of Gaussian process (GP) regression. We consider continuous-discrete estimation problems wherein a trajectory is viewed as a one-dimensional GP, with time as the independent variable. Our continuous-time prior can be defined by any nonlinear, time-varying stochastic differential equation driven by white noise; this allows the possibility of smoothing our trajectory estimates using a variety of vehicle dynamics models (e.g., `constant-velocity'). We show that this class of prior results in an inverse kernel matrix (i.e., covariance matrix between all pairs of measurement times) that is exactly sparse (block-tridiagonal) and that this can be exploited to carry out GP regression (and interpolation) very efficiently. When the prior is based on a linear, time-varying stochastic differential equation and the measurement model is also linear, this GP approach is equivalent to classical, discrete-time smoothing (at the measurement times); when a nonlinearity is present, we iterate over the whole trajectory to maximize accuracy. We test the approach experimentally on a simultaneous trajectory estimation and mapping problem using a mobile robot dataset.