Robotics: Science and Systems X

Timothy D. Barfoot, Chi Hay Tong, Simo Särkkä, D Alvarez, Luengo, Lawrence, T Bailey, H Durrant-Whyte, C Bibby, I D Reid, M Bosse, Zlot, A J Davison, N D Molton, Stasse, M P Deisenroth, R Turner, M Huber, U D Hanebeck, C E Rasmussen, F Dellaert, M Kaess, H J Dong, H R M Eustice, J J Singh, Leonard, D Ferris, Hhnel, Fox, T D P T Furgale, G Barfoot, Sibley, O Grau, J Pansiot, J Hartikainen, Srkk, M Hartikainen, Seppnen, Hedborg, M Forssn, Felsberg, Ringaby, Ranganathan, H Kaess, R Johannsson, V Roberts, J J Ila, R E Kalman, R S Bucy, J Ko, H Lindgren, Rue, Lindstrm, A Lovegrove, G Patron-Perez, F Lu, E Milios, P S Maybeck, G Newman, M Smith, Cummins, C Harrison, I Mei, R Posner, D Shade, L Schroeter, W Murphy, D Churchill, Cole, Sarmavuori, Solin, G Matthies, Sukhatme, R C Smith, P CheesemanOpenalex Unknown 2014 · DOI · OpenAlex

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 linear, 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. Though the prior is continuous, we consider measurements to occur at discrete times. When the measurement model is also linear, this GP approach is equivalent to classical, discrete-time smoothing (at the measurement times). When the measurement model is nonlinear, we iterate over the whole trajectory (as is common in vision and robotics) to maximize accuracy. We test the approach experimentally on a simultaneous trajectory estimation and mapping problem using a mobile robot dataset.

Introduction

Traditional robotics applications typically estimate a robot's trajectory at discrete time intervals. However, real-world sensors—such as rolling-shutter cameras or spinning lidars—often operate asynchronously or involve "scanning-while-moving" effects that are fundamentally continuous-time phenomena. While Gaussian Processes (GPs) offer a robust framework for continuous-time estimation, their adoption has been hindered by computational complexity; standard GP regression requires inverting a dense kernel matrix, leading to $O(M^3)$ scaling for $M$ measurements. This makes them impractical for long robot trajectories.

This paper bridges the gap between GP regression and classical state estimation. By modeling trajectories via linear stochastic differential equations (SDEs), the authors demonstrate that the resulting inverse kernel matrices are "exactly sparse" (block-tridiagonal). This insight allows continuous-time trajectory estimation to scale linearly, matching the efficiency of traditional discrete-time smoothing while maintaining the flexibility of GPs.

Methodology: Sparsity in Continuous-Time Priors

The core contribution lies in the choice of the GP prior. If the prior is generated by a Linear, Time-Varying (LTV) SDE driven by white noise, the precision matrix (inverse covariance) becomes block-tridiagonal. This structure is a direct consequence of the Markov property: by including derivatives (like velocity) in the state representation, the state at any given time depends only on its immediate predecessor.

Continuous-time Gaussian process trajectory with asynchronous measurement times and an arbitrary query time $\tau$.

This formulation, termed Simultaneous Trajectory Estimation and Mapping (STEAM), allows for efficient interpolation. Querying the state at an arbitrary time $\tau$ between measurements becomes an $O(1)$ operation. For nonlinear measurement models, the authors employ an iterative Gauss-Newton optimization that preserves this sparsity at every step. A major insight is that re-introducing variables like velocity—which are often marginalized out—actually simplifies the problem by preserving the Markov property.

Experimental Evaluation

The proposed approach was validated using a mobile robot dataset in an indoor planar environment containing 17 plastic-tube landmarks. The robot utilized a laser rangefinder for landmark detection and wheel odometry, with ground truth provided by a Vicon motion capture system. Measurements were processed at 1Hz, while the trajectory was queried at 10Hz to test interpolation performance.

Results and Performance

The experimental results confirm that the GP-Traj-Sparse method achieves linear scaling $O(M+N)$ for $M$ measurements and $N$ queries. This is a significant improvement over the $O(M^3)$ scaling of dense GP methods.

Computational performance comparison across dataset length, showing (a) kernel construction, (b) optimization, (c) interpolation, and (d) total computation time.

As shown in the performance metrics, the sparse implementation is orders of magnitude faster for long trajectories. For a 1200-second dataset, optimization took less than a second per iteration, compared to nearly 100 seconds for the dense version. Crucially, this speedup does not sacrifice accuracy; the method maintains the same precision as dense GPs while offering better numerical stability from the direct construction of the inverse kernel.

Conclusion and Future Work

By re-introducing variables like velocity to maintain the Markov property, the authors show that batch GP regression is mathematically equivalent to classical discrete-time smoothing at measurement points, but with a principled, nonparametric method for interpolation. Future work will explore extending this framework to nonlinear SDEs and more complex vehicle dynamics to further enhance real-time SLAM capabilities.

Summary updated

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 linear, 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. Though the prior is continuous, we consider measurements to occur at discrete times. When the measurement model is also linear, this GP approach is equivalent to classical, discrete-time smoothing (at the measurement times). When the measurement model is nonlinear, we iterate over the whole trajectory (as is common in vision and robotics) to maximize accuracy. We test the approach experimentally on a simultaneous trajectory estimation and mapping problem using a mobile robot dataset.
BibTeX
@inproceedings{barfoot2014robotics,
  author = {Timothy D. Barfoot and Chi Hay Tong and Simo Särkkä and D Alvarez and Luengo and Lawrence and T Bailey and H Durrant-Whyte and C Bibby and I D Reid and M Bosse and Zlot and A J Davison and I D Reid and N D Molton and Stasse and M P Deisenroth and R Turner and M Huber and U D Hanebeck and C E Rasmussen and F Dellaert and M Kaess and H J Dong and T D Barfoot and H Durrant-Whyte and T Bailey and H F Durrant-Whyte and H R M Eustice and J J Singh and Leonard and D Ferris and Hhnel and Fox and D Ferris and Fox and Lawrence and T D P T Furgale and G Barfoot and Sibley and O Grau and J Pansiot and J Hartikainen and Srkk and M Hartikainen and Seppnen and Srkk and Hedborg and M Forssn and Felsberg and Ringaby and Kaess and Ranganathan and Dellaert and H Kaess and R Johannsson and V Roberts and J J Ila and Leonard and Dellaert and R E Kalman and R E Kalman and R S Bucy and J Ko and D Fox and J Ko and D Fox and Lawrence and H Lindgren and Rue and Lindstrm and A Lovegrove and G Patron-Perez and Sibley and F Lu and E Milios and P S Maybeck and G Newman and M Sibley and M Smith and Cummins and C Harrison and I Mei and R Posner and D Shade and L Schroeter and W Murphy and D Churchill and Cole and Reid and S Srkk and Sarmavuori and Srkk and Solin and Hartikainen and L Sibley and G Matthies and Sukhatme and R C Smith and P Cheeseman},
  title = {{Robotics: Science and Systems X}},
  booktitle = {Openalex Unknown},
  year = {2014},
  doi = {10.15607/rss.2014.x},
}

Loading related content...

Figure