Gaussian Variational Inference With Non-Gaussian Factors for State Estimation: A UWB Localization Case Study

Andrew Stirling, Mykola Lukashchuk, Dmitry Bagaev, Wouter M. Kouw, James Richard Forbes

Abstract

Traditional robot state estimation often relies on Maximum A-Posteriori (MAP) estimation under Gaussian noise assumptions. However, sensors like Ultra-Wideband (UWB) frequently exhibit non-Gaussian, skewed error distributions due to Non-Line-of-Sight (NLOS) and multipath effects. This paper extends the Exactly Sparse Gaussian Variational Inference (ESGVI) algorithm to operate on matrix Lie groups and incorporate robust, asymmetric likelihood functions. By approximating the full posterior distribution rather than just the mode, the proposed method demonstrates enhanced robustness and accuracy in cluttered indoor environments.

Introduction

Most robotic systems estimate pose using MAP estimation, which identifies the most likely single point (the mode) of a probability distribution. This approach faces two primary challenges: the "Gaussian Trap," where sensor noise is incorrectly assumed to be symmetric and bell-shaped, and the "Geometry Gap," where orientation is treated as Euclidean space rather than on its natural geometric manifold. In real-world UWB applications, signal delays caused by obstacles result in heavily skewed error distributions that standard Gaussian models fail to capture.

Methodology

The researchers introduced three key extensions to the ESGVI framework to address these challenges:

  1. Manifold Integration: The update rules were reformulated for matrix Lie groups (specifically $SE(2)$ for planar robots), ensuring pose estimation remains on the proper geometric manifold and avoiding mathematical singularities.
  2. Non-Gaussian Factors: Robust, asymmetric likelihood functions, such as Skew-Laplace and asymmetric Cauchy distributions, were integrated directly into the variational inference process. These models explicitly account for the positive bias (delays) typical of UWB measurements in NLOS conditions.
  3. Derivative-Free Optimization: Utilizing Stein’s Lemma and cubature methods, the algorithm computes updates via expectations. This allows for the seamless integration of complex noise models without the need for manual gradient derivations.
Histogram of UWB measurement errors showing the skewness caused by NLOS and multipath effects, along with various distribution fits.

Experimental Evaluation

The approach was validated through both simulation and physical experiments:

  • Simulation: 50 Monte Carlo trials were conducted with 25% of UWB measurements corrupted by uniform noise to simulate NLOS conditions.
  • Real-World Trials: A Clearpath Husky robot equipped with UWB tags navigated a 40 $m^2$ area containing metal, wood, and foam obstacles. Ground truth was established using a high-precision Vicon motion capture system.

Results and Discussion

Experimental results indicate that considering the full posterior distribution via Variational Inference provides significant resilience against outliers compared to point-based MAP methods.

Comparison of estimated trajectories against ground truth (GT) for MAP-C, MAP-GMM, and ESGVI methods in the real-world experiment.

Key findings include:

  • Improved Accuracy: ESGVI achieved a translational Root-Mean-Squared Error (RMSE) of 0.046 m in simulations, matching the best-performing Gaussian Mixture Models (GMM) and outperforming standard robust MAP (0.050 m).
  • Consistency: The method maintained high consistency, as measured by the average Normalized Estimation Error Squared (aNEES), performing comparably to the best MAP-GMM variants.
  • Generalization: While GMMs struggled when NLOS proportions varied across trajectories, the ESGVI approach with Skew-Laplace factors remained consistent across different environments.

Conclusion

The integration of non-Gaussian factors into a manifold-aware variational inference framework significantly improves robot localization in challenging environments. While the current cubature-based implementation is computationally more intensive than MAP methods—limiting it to offline use—future work will focus on optimization and faster line-search techniques to enable real-time performance. The authors have released their Python implementation as open-source to encourage further community development.

Summary updated

Abstract

This letter extends the exactly sparse Gaussian variational inference (ESGVI) algorithm for state estimation in two complementary directions. First, ESGVI is generalized to operate on matrix Lie groups, enabling the estimation of states with orientation components while respecting the underlying group structure. Second, factors are introduced to accommodate heavy-tailed and skewed noise distributions, as commonly encountered in ultra-wideband (UWB) localization due to non-line-of-sight (NLOS) and multipath effects. Both extensions are shown to integrate naturally within the ESGVI framework while preserving its sparse and derivative-free structure. The proposed approach is validated in a UWB localization experiment with NLOS-rich measurements, demonstrating improved accuracy and comparable consistency. Finally, a <sc xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">Python</small> implementation within a factor-graph-based estimation framework is made open-source to support broader research use.
BibTeX
@article{stirling2026gaussian,
  author = {Andrew Stirling and Mykola Lukashchuk and Dmitry Bagaev and Wouter M. Kouw and James Richard Forbes},
  title = {{Gaussian Variational Inference With Non-Gaussian Factors for State Estimation: A UWB Localization Case Study}},
  journal = {IEEE Robotics Autom. Lett.},
  year = {2026},
  doi = {10.1109/lra.2026.3653370},
}
Figure