Batch Continuous time Trajectory Estimation as Exactly Sparse Gaussian Process Regression

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.

References

• Alvarez, M., Luengo, D., & Lawrence, N. (2009). Latent force models. In Proceedings of the international conference on artificial intelligence and statistics (AISTATS).

• Bailey, T., & Durrant-Whyte, H. (2006). SLAM: Part II state of the art. IEEE RAM, 13(3), 108–117.

  Google Scholar

• Barfoot, T. D., Tong, C. H., & Särkkä, S. (2014). Batch continuous-time trajectory estimation as exactly sparse gaussian process regression. In Proceedings of robotics: Science and systems (RSS). Berkeley.

• Bell, B. M. (1994). The iterated Kalman smoother as a Gauss-Newton method. SIAM Journal on Optimization, 4(3), 626–636.

  MATH  MathSciNet  Article  Google Scholar

• Bibby, C., & Reid, I. D. (2010). A hybrid SLAM representation for dynamic marine environments. In Proceedings of ICRA.

• Bosse, M., & Zlot, R. (2009). Continuous 3D scan-matching with a spinning 2D laser. In Proceedings of ICRA.

• Brown, D. C. (1958). A solution to the general problem of multiple station analytical stereotriangulation. RCA-MTP data reduction technical report no. 43, Patrick Airforce Base.

• Davison, A. J., Reid, I. D., Molton, N. D., & Stasse, O. (2007). MonoSLAM: Real-time single camera SLAM. IEEE Transactions on PAMI, 29(6), 1052–1067.

  Article  Google Scholar

• Deisenroth, M. P., Turner, R., Huber, M., Hanebeck, U. D., & Rasmussen, C. E. (2012). Robust filtering and smoothing with Gaussian processes. IEEE Transactions on Automatic Control, 57, 1865–1871.

  MathSciNet  Article  Google Scholar

• Dellaert, F., & Kaess, M. (2006). Square root SAM: Simultaneous localization and mapping via square root information smoothing. IJRR, 25(12), 1181–1204.

  MATH  Google Scholar

• Dong, H. J., & Barfoot, T. D. (2012). Lighting-invariant visual odometry using lidar intensity imagery and pose interpolation. In Proceedings of field and service robotics

• Durrant-Whyte, H., & Bailey, T. (2006). SLAM: Part I essential algorithms. IEEE RAM, 11(3), 99–110.

  Google Scholar

• Durrant-Whyte, H. F. (1988). Uncertain geometry in robotics. IEEE Journal of Robotics and Automation, 4(1), 23–31.

  Article  Google Scholar

• Eustice, R. M., Singh, H., & Leonard, J. J. (2006). Exactly sparse delayed-state filters for view-based SLAM. IEEE TRO, 22(6), 1100–1114.

  Google Scholar

• Ferris, B., Fox, D., & Lawrence, N. (2007). Wifi-SLAM using Gaussian process latent variable models. In Proceedings of IJCAI.

• Ferris, B., Hähnel, D., & Fox, D. (2006). Gaussian processes for signal strength-based localization. In Proceedings of RSS.

• Furgale, P. T., Barfoot, T. D., & Sibley, G. (2012). Continuous-time batch estimation using temporal basis functions. In Proceedings of ICRA.

• Grau, O., & Pansiot, J. (2012). Motion and velocity estimation of rolling shutter cameras. In Proceedings of the 9th European conference on visual media production (pp. 94–98).

• Hartikainen, J., & Särkkä, S. (2010). Kalman filtering and smoothing solutions to temporal Gaussian process regression models. In Proceedings of the IEEE international work on machine learning for signal processing.

• Hartikainen, J., Seppänen, M., & Särkkä, S. (2012). State-space inference for non-linear latent force models with application to satellite orbit prediction. In Proceedings of ICML.

• Hedborg, J., Forssén, P., Felsberg, M., & Ringaby, E. (2012). Rolling shutter bundle adjustment. In Proceedings of CVPR.

• Jazwinski, A. H. (1970). Stochastic processes and filtering theory. New York: Academic Press.

  MATH  Google Scholar

• Jumarie, G. (1990). Nonlinear filtering: A weighted mean squares approach and a Bayesian one via the maximum entropy principle. Signal Processing, 21(4), 323–338.

  MATH  Article  Google Scholar

• Kaess, M., Johannsson, H., Roberts, R., Ila, V., Leonard, J. J., & Dellaert, F. (2012). iSAM2: Incremental smoothing and mapping using the Bayes tree. IJRR, 31(2), 217–236.

  Google Scholar

• Kaess, M., Ranganathan, A., & Dellaert, R. (2008). iSAM: Incremental smoothing and mapping. IEEE TRO, 24(6), 1365–1378.

  Google Scholar

• Kalman, R. E. (1960). A new approach to linear filtering and prediction problems. Transactions of the ASME: Journal of Basic Engineering, 82(Series D), 35–45.

  Article  Google Scholar

• Kalman, R. E., & Bucy, R. S. (1961). New results in linear filtering and prediction theory. Transactions of the ASME: Journal of Basic Engineering, 83(3), 95–108.

  MathSciNet  Article  Google Scholar

• Ko, J., & Fox, D. (2009). GP-Bayes filters: Bayesian filtering using Gaussian process prediction and observation models. Autonomous Robots, 27(1), 75–90.

  Article  Google Scholar

• Ko, J., & Fox, D. (2011). Learning GP-BayesFilters via Gaussian process latent variable models. Autonomous Robots, 30(1), 3–23.

  Article  Google Scholar

• Lawrence, N. (2003). Gaussian process latent variable models for visualization of high dimensional data. In Proceedings of NIPS.

• Lindgren, F., Rue, H., & Lindström, J. (2011). An explicit link between Gaussian fields and Gaussian Markov random fields: The stochastic partial differential equation approach. Journal of the Royal Statistical Society: Series B, 73(4), 423–498.

  MATH  MathSciNet  Article  Google Scholar

• Lovegrove, S., Patron-Perez, A., & Sibley, G. (2013). Spline fusion: A continuous-time representation for visual-inertial fusion with application to rolling shutter cameras. In Proceedings of BMVC.

• Lu, F., & Milios, E. (1997). Globally consistent range scan alignment for environment mapping. Autonomous Robots, 4(4), 333–349.

  Article  Google Scholar

• Maybeck, P. S. (1979). Stochastic models, estimation, and control volume 141 of mathematics in science and engineering. New York: Academic Press Inc.

  Google Scholar

• Newman, P., Sibley, G., Smith, M., Cummins, M., Harrison, A., Mei, C., et al. (2009). Navigating, recognising and describing urban spaces with vision and laser. IJRR, 28(11–12), 1406–1433.

  Google Scholar

• Oth, L., Furgale, P. T., Kneip, L., & Siegwart, R. (2013). Rolling shutter camera calibration. In Proceedings of The IEEE international conference on computer vision and pattern recognition (CVPR). Portland.

• Rasmussen, C. E., & Williams, C. K. I. (2006). Gaussian processes for machine learning. Cambridge: MIT Press.

  MATH  Google Scholar

• Särkkä, S. (2006). Recursive Bayesian inference on stochastic differential equations (Ph.D. thesis, Helsinki University of Technology, 2006).

• Särkkä, S. (2013). Bayesian filtering and smoothing. Cambridge: Cambridge University Press.

  MATH  Google Scholar

• Särkkä, S., & Sarmavuori, J. (2013). Gaussian filtering and smoothing for continuous-discrete dynamic systems. Signal Processing, 93(2), 500–510.

  Article  Google Scholar

• Särkkä, S., Solin, A., & Hartikainen, J. (2013). Spatiotemporal learning via infinite-dimensional Bayesian filtering and smoothing: A look at Gaussian process regression through Kalman filtering. IEEE Signal Processing Magazine, 30(4), 51–61.

  Article  Google Scholar

• Sibley, G., Matthies, L., & Sukhatme, G. (2010). Sliding window filter with application to planetary landing. Journal of Field Robotics, 27(5), 587–608.

  Article  Google Scholar

• Smith, R. C., & Cheeseman, P. (1986). On the representation and estimation of spatial uncertainty. IJRR, 5(4), 56–68.

  Google Scholar

• Smith, R. C., Self, M., & Cheeseman, P. (1990). Estimating uncertain spatial relationships in robotics. In I. J. Cox & G. T. Wilfong (Eds.), Autonomous Robot Vehicles (pp. 167–193). New York: Springer.

  Chapter  Google Scholar

• Solin, A. & Särkkä, S. (2014). Explicit link between periodic covariance functions and state space models. In Proceedings of the international conference on artificial intelligence and statistics (AISTATS).

• Stengel, R. F. (1994). Optimal control and estimation. Mineola: Dover Publications Inc.

  MATH  Google Scholar

• Strasdat, H., Montiel, J. M. M., & Davison, A. J. (2010). Real-time monocular SLAM: Why filter?. In Proceedings of ICRA.

• Thrun, S., & Montemerlo, M. (2006). The graph SLAM algorithm with applications to large-scale mapping of urban structures. IJRR, 25(5–6), 403–429.

  Google Scholar

• Tong, C. H., Furgale, P., & Barfoot, T. D. (2012). Gaussian process Gauss-Newton: Non-parametric state estimation. In Proceedings of the 9th conference on computer and robot vision (pp. 206–213).

• Tong, C. H., Furgale, P. T., & Barfoot, T. D. (2013). Gaussian process Gauss-Newton for non-parametric simultaneous localization and mapping. IJRR, 32(5), 507–525.

  Google Scholar

• Triggs, B., McLauchlan, P., Hartley, R., & Fitzgibbon, A. (2000). Bundle adjustment: A modern synthesis. Lecture notes in computer science. In B. Triggs, A. Zisserman, & R. Szeliski (Eds.), Vision algorithms: Theory and practice (pp. 298–372). Berlin/Heidelberg: Springer.

  Chapter  Google Scholar

• Walter, M. R., Eustice, R. M., & Leonard, J. J. (2007). Exactly sparse extended information filters for feature-based SLAM. IJRR, 26(4), 335–359.

  Google Scholar

Download references

Acknowledgments

Thanks to Dr. Alastair Harrison at Oxford who asked the all-important question: how can the GP estimation approach (Tong et al. 2013) be related to factor graphs? This work was supported by the Canada Research Chair Program, the Natural Sciences and Engineering Research Council of Canada, and the Academy of Finland.

Author information

Authors and Affiliations

1. Autonomous Space Robotics Lab, University of Toronto Institute for Aerospace Studies, North York, Canada

   Sean Anderson & Timothy D. Barfoot

2. Mobile Robotics Group, University of Oxford, Oxford, UK

   Chi Hay Tong

3. Department of Electrical Engineering and Automation, Aalto University, Espoo, Finland

   Simo Särkkä

Corresponding author

Correspondence to Sean Anderson.

Additional information

This is one of several papers published in Autonomous Robots comprising the "Special Issue on Robotics Science and Systems".

About this article

Cite this article

Anderson, S., Barfoot, T.D., Tong, C.H. et al. Batch nonlinear continuous-time trajectory estimation as exactly sparse Gaussian process regression. Auton Robot 39, 221–238 (2015). https://doi.org/10.1007/s10514-015-9455-y

Download citation

• Received: 20 November 2014

• Accepted: 02 July 2015

• Published: 26 July 2015

• Issue Date: October 2015

• DOI : https://doi.org/10.1007/s10514-015-9455-y

Keywords

• State estimation
• Localization
• Continuous time
• Gaussian process regression

jonespaided.blogspot.com

Source: https://link.springer.com/article/10.1007/s10514-015-9455-y

Share this post