Fully-Probabilistic Terrain Modelling and Localization With Stochastic Variational Gaussian Process Maps

Ignacio Torroba, Christopher Iliffe Sprague, John Folkesson

TL;DR

This paper introduces a scalable, fully-probabilistic framework for underwater terrain mapping that accounts for the inherent uncertainty in a robot's position. By combining Stochastic Variational Gaussian Processes (SVGPs) with Monte Carlo sampling, the authors create maps that are not only more accurate at predicting unseen areas but also provide more reliable uncertainty estimates for autonomous underwater vehicle (AUV) localization.

What Problem Does This Paper Address?

Underwater mapping is a "noisy" business. AUVs lack GPS signals, meaning they must rely on dead reckoning (DR), which accumulates error over time. When an AUV records a depth measurement using sonar, there is uncertainty not just in the depth reading itself, but in the coordinates (x, y) where that reading was taken.

Standard mapping techniques often treat these input coordinates as deterministic (fixed). While Gaussian Processes (GPs) are excellent at modeling surface uncertainty, traditional "Uncertain Input" GP methods are mathematically cumbersome, restricted to specific noise types (usually Gaussian), and don't scale well to the millions of data points generated during a typical mission. This paper seeks a flexible, efficient way to build large-scale maps that "know" how uncertain their own inputs were.

Methods

The authors propose a two-stage approach:

  1. Uncertainty Propagation: They use the sigmapoint method (similar to an Unscented Kalman Filter) to propagate the AUV’s navigation uncertainty and sensor noise into the 3D coordinates of the sonar beams. This results in "Uncertain Inputs" (UIs)—each data point is treated as a distribution rather than a single coordinate.
  2. SVGP with Monte Carlo Sampling: To make training tractable for millions of points, they use Stochastic Variational Gaussian Processes (SVGPs). The "secret sauce" is how they handle the UIs: during each training minibatch, they draw a random sample from the UI distributions (Monte Carlo sampling). As training progresses, the model naturally learns to incorporate this input noise without needing complex analytical reformulations of the GP kernel.

Finally, they implement a Particle Filter (PF) localization algorithm that uses these SVGP maps to estimate the AUV's position in real-time, leveraging the map's probabilistic nature to weight position hypotheses.

Datasets

The researchers validated their approach using a real-world dataset:

  • Hugin 3000 AUV Survey: Collected off the Swedish west coast.
  • Scale: Approximately 4.038 million multibeam echosounder (MBES) beams covering 1.4 $km^2$.
  • Mission Duration: ~12.7 hours with a 30.5 km trajectory.
  • Ground Truth: The original high-quality DR data was used as a baseline, which was then intentionally corrupted with synthetic noise to test the robustness of the mapping algorithms.

Key Insights and Ideas

  • Prediction vs. Reconstruction: Interestingly, using UIs doesn't significantly change the error in reconstructing areas the robot has already seen. However, it drastically improves the prediction of unseen areas. The model learns a more generalized "smoothness" that better represents the true terrain when the input data is shaky.
  • Scalability via Sampling: By using Monte Carlo sampling within the SVGP framework, the authors bypassed the need for the "Gauss-Hermite quadrature" used in previous works. This makes the system agnostic to the type of noise (it doesn't have to be Gaussian) and much easier to implement using standard machine learning libraries like GPyTorch.
  • Better Localization "Feel": Because the UI-SVGP map provides a more "honest" measure of variance, the Particle Filter performs better. It doesn't get "overconfident" about noisy map data, leading to more accurate vehicle pose estimates.

Key Results

  • Improved Accuracy: In predictive tasks (estimating depth in unsampled areas), the UI-SVGP consistently outperformed the Deterministic Input (DI) version. For example, at a specific noise level, the Prediction RMSE dropped from 0.766m (DI) to 0.566m (UI).
  • Localization Gains: When running a Particle Filter on an entirely predicted area of the seabed, the UI-SVGP map resulted in a lower position error (10.71m) compared to the DI-SVGP map (11.88m) and raw dead reckoning (12.32m).
  • Computational Trade-off: The UI approach required a slight increase in training iterations (roughly 4-6% more) to converge, which the authors deem a negligible cost for the gain in accuracy.

Limitations and Future Work

  • Inducing Point Selection: The number of "inducing points" (a subset of points used to represent the GP) was set manually to 1,000 based on hardware limits. The authors suggest that automatically learning this number as a hyperparameter would be a valuable next step for online (real-time) applications.
  • Generalization: While tested on a large bathymetric dataset, the authors note that further research is needed to see how this approach generalizes to different types of seabed topography.
  • Online Implementation: The current study focused on post-processing and validation; moving this into a fully online, "on-the-fly" mapping system for AUVs remains a promising future direction.

Summary updated

Abstract

Gaussian processes (GPs) are becoming a standard tool to build terrain representations thanks to their capacity to model map uncertainty. This effectively yields a reliability measure of the areas of the map, which can be directly utilized by Bayes filtering algorithms in robot localization problems. A key factor is that this map uncertainty can incorporate the noise intrinsic to the terrain surveying process through the GPs ability to train on uncertain inputs (UIs). However, existing techniques to build GP maps with UIs in a tractable manner are restricted in the form and degree of the input noise. In this letter, we propose a flexible and efficient framework to build large-scale GP maps with UIs based on Stochastic Variational GPs and Monte Carlo sampling of the UIs distributions. We validate our mapping approach on a large bathymetric survey collected with an autonomous underwater vehicle (AUV) and analyze its performance against the use of deterministic inputs (DI). Finally, we show how using UI SVGP maps yields more accurate particle filter localization results than DI SVGP on a real AUV mission over an entirely predicted area.
BibTeX
@article{torroba2022fully,
  author = {Ignacio Torroba and Christopher Iliffe Sprague and John Folkesson},
  title = {{Fully-Probabilistic Terrain Modelling and Localization With Stochastic Variational Gaussian Process Maps}},
  journal = {IEEE Robotics Autom. Lett.},
  year = {2022},
  doi = {10.1109/lra.2022.3182807},
}
Figure