Batch Nonlinear Continuous-Time Trajectory Estimation as Exactly Sparse Gaussian Process Regression
This work addresses trajectory estimation for robotics by providing an efficient, sparse GP method that can handle nonlinear dynamics, though it is incremental as it builds on existing GP and smoothing techniques.
The paper tackles batch state estimation by framing it as Gaussian process regression with a continuous-time prior defined by nonlinear stochastic differential equations, showing that the inverse kernel matrix becomes exactly sparse and block-tridiagonal, enabling efficient computation. It demonstrates this approach on a mobile robot dataset for simultaneous trajectory estimation and mapping, achieving competitive performance.
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.