Spatially scalable recursive estimation of Gaussian process terrain maps using local basis functions

TL;DR

This work tackles the computational bottlenecks of online Gaussian process terrain mapping for SLAM in GNSS-denied environments. It introduces a spatially scalable, recursive GP estimator that uses a global grid of finite-support basis functions but restricts computation to a local subset around each prediction, implemented via an information filter and integrated into an EKF for magnetic-field SLAM. Key contributions include (i) a local-subset map querying and updating scheme with bounded complexity, (ii) a practical EKF-Mag-SLAM formulation that preserves sparsity, and (iii) extensive experiments showing substantial speedups over state-of-the-art baselines while maintaining accuracy on 1D, 2D, and global-scale geospatial tasks. The approach significantly enables real-time online large-scale terrain mapping and navigation in GNSS-denied settings, with broad applicability to robotics and autonomous systems.

Abstract

When an agent, person, vehicle or robot is moving through an unknown environment without GNSS signals, online mapping of nonlinear terrains can be used to improve position estimates when the agent returns to a previously mapped area. Mapping algorithms using online Gaussian process (GP) regression are commonly integrated in algorithms for simultaneous localisation and mapping (SLAM). However, GP mapping algorithms have increasing computational demands as the mapped area expands relative to spatial field variations. This is due to the need for estimating an increasing amount of map parameters as the area of the map grows. Contrary to this, we propose a recursive GP mapping estimation algorithm which uses local basis functions in an information filter to achieve spatial scalability. Our proposed approximation employs a global grid of finite support basis functions but restricts computations to a localized subset around each prediction point. As our proposed algorithm is recursive, it can naturally be incorporated into existing algorithms that uses Gaussian process maps for SLAM. Incorporating our proposed algorithm into an extended Kalman filter (EKF) for magnetic field SLAM reduces the overall computational complexity of the algorithm. We show experimentally that our algorithm is faster than existing methods when the mapped area is large and the map is based on many measurements, both for recursive mapping tasks and for magnetic field SLAM.

Figures (7)

• Figure 1: A locally reconstructed approximation (indicated by the color of the heatmap) of a simulated large, nonlinear geospatial field (indicated with gray level curves) based on a local subset (marked with the black circles) of a global grid of basis functions (marked with the gray circles).
• Figure 2: Sparsity patterns illustrating which entries $i,j$ in the information matrix correspond to pairs of basis function locations $x_i,x_j$ that are closer than $2r^{\star}$ according to the infinity norm (dark blue) and which are not (light blue). The patterns arise from the ordering of the indexes of the basis functions, relative to their locations along each of the three dimensions.
• Figure 3: Sparsity pattern illustration of the information matrix for the full state consisting of the position, orientation and magnetic field. The dark blue entries indicate entries which are necessary to compute with our assumptions, and the light blue entries indicate values that are not necessary to compute. The first sparsity pattern indicate the values we need associated with the position and orientation (the first 6 states in the full state-space). The second sparsity pattern has a dark blue color in the entries in the set $S_{\forall \star}$ of all entries that can possibly be necessary to make a map prediction in any location. The last sparsity pattern is the union of these two sets.
• Figure 4: KL divergence (KLD) between the full GP posterior, and approximations with various local domain sizes $r$, trained on the audio dataset. The error bars indicate the average deviation above and below the mean, respectively, after 100 repeated experiments with 100 randomly sampled measurements from the training set.
• Figure 5: Online inference time of the sound map for a growing domain size. We compare our proposed method (with various local domain sizes $r$) to the inducing input approximation using inducing inputs on a grid, the Hilbert space basis function approximation, and SKI. All methods were run using the same amount of basis functions. The error bars indicate the average deviation above and below the mean after 100 repeated experiments, respectively.