Variational Inference with Parameter Learning Applied to Vehicle Trajectory Estimation

Jeremy N. Wong, David J. Yoon, Angela P. Schoellig, Timothy D. Barfoot

Abstract

We present a method for parameter learning in a Gaussian variational inference setting using only noisy measurements, without requiring groundtruth data. This approach is demonstrated within the context of vehicle trajectory estimation by extending the Exactly Sparse Gaussian Variational Inference (ESGVI) framework. Our contribution involves learning the covariances for motion and sensor models, specifically utilizing a white-noise-on-acceleration motion model and an Inverse-Wishart prior for measurement covariances. Tested on a 36 km lidar dataset and standard pose graph optimization benchmarks, the method achieves high-quality state estimates and demonstrates significant robustness to outliers and false loop closures.

Introduction

Probabilistic state estimation is fundamental to robot navigation, yet it relies heavily on accurately tuned noise models. Traditionally, these parameters are either hand-tuned or learned using expensive groundtruth data. This paper addresses the "chicken-and-egg" problem of estimating a robot's path while simultaneously learning the noise characteristics of its sensors and actuators. By extending the ESGVI framework, we enable the system to learn these parameters directly from noisy observations, making the process more adaptable to new platforms and resilient to real-world data imperfections.

Methodology: ESGVI and Parameter Learning

The proposed framework combines Exactly Sparse Gaussian Variational Inference (ESGVI) with an Expectation-Maximization (EM) loop. In the E-step, the algorithm estimates the most likely robot trajectory (latent state) given fixed noise parameters. In the M-step, the trajectory is held fixed while the noise parameters—specifically the covariances—are optimized to better fit the observed data.

Factor graph representation of the proposed variational inference framework.

A key innovation is the use of an Inverse-Wishart (IW) prior for sensor noise. Rather than assuming a static covariance, the system treats the covariance as a random variable. This allows for automatic outlier rejection; if a measurement is significantly inconsistent with the estimated path, the system reduces its influence. To prevent the optimization from collapsing (uncertainty reaching zero), the authors implement a volume constraint on the determinant of the scale matrix.

Experimental Setup and Results

The method was validated on a 36 km lidar dataset (Route A for training, Route B for testing) and the Bicocca 25b dataset for pose graph optimization.

Performance and Robustness

  • Groundtruth Independence: Training without groundtruth yielded a translational error of 0.2335m, performing slightly better than training with incomplete groundtruth (0.2368m).
  • Outlier Mastery: In scenarios with 5% artificial outliers, the proposed IW-prior method maintained an error of ~0.23m, whereas standard static models failed completely (errors > 5m).
  • SLAM Stability: The framework successfully resolved pose graphs containing numerous false loop closures that typically cause divergence in standard SLAM systems.
Robustness to measurement outliers: map view (left) and error concentration (right).

Discussion and Future Work

The results demonstrate that the internal consistency of a factor graph is sufficient to learn noise parameters without a perfect reference. While the system currently requires manual setting of two metaparameters (degrees-of-freedom and the determinant constraint), future work aims to automate this process. Furthermore, the authors suggest this framework could be expanded to learn complex robot models represented by Deep Neural Networks (DNNs), bridging the gap between classical estimation and modern deep learning.

Summary updated

Abstract

We present parameter learning in a Gaussian variational inference setting using only noisy measurements (i.e., no groundtruth). This is demonstrated in the context of vehicle trajectory estimation, although the method we propose is general. The paper extends the Exactly Sparse Gaussian Variational Inference (ESGVI) framework, which has previously been used for large-scale nonlinear batch state estimation. Our contribution is to additionally learn parameters of our system models (which may be difficult to choose in practice) within the ESGVI framework. In this paper, we learn the covariances for the motion and sensor models used within vehicle trajectory estimation. Specifically, we learn the parameters of a white-noise-on-acceleration motion model and the parameters of an Inverse-Wishart prior over measurement covariances for our sensor model. We demonstrate our technique using a 36~km dataset consisting of a car using lidar to localize against a high-definition map; we learn the parameters on a training section of the data and then show that we achieve high-quality state estimates on a test section, even in the presence of outliers. Lastly, we show that our framework can be used to solve pose graph optimization even with many false loop closures.
BibTeX
@article{wong2020variational,
  author = {Jeremy N. Wong and David J. Yoon and Angela P. Schoellig and Timothy D. Barfoot},
  title = {{Variational Inference with Parameter Learning Applied to Vehicle Trajectory Estimation}},
  journal = {IEEE Robotics Autom. Lett.},
  year = {2020},
  doi = {10.1109/lra.2020.3007381},
}
Figure