Towards Real-Time Non-Gaussian SLAM for Underdetermined Navigation

Abstract

This paper presents a method for processing sparse, non-Gaussian multimodal data in a simultaneous localization and mapping (SLAM) framework using factor graphs. Our approach demonstrates the feasibility of using a sum-product inference strategy to recover functional belief marginals from highly non-Gaussian situations, relaxing the prolific unimodal Gaussian assumption. The method is more focused than conventional multi-hypothesis approaches, but still captures dominant modes via multi-modality. The proposed algorithm exists in a trade space that spans the anticipated uncertainty of measurement data, task-specific performance, sensor quality, and computational cost. This work leverages several major algorithm design constructs, including clique recycling, to put an upper bound on the allowable computational expense – a major challenge in non-parametric methods. To better demonstrate robustness, experimental results show the feasibility of the method on at least two of four major sources of non-Gaussian behavior: i) the first introduces a canonical range-only problem which is always underdetermined although composed exclusively from Gaussian measurements; ii) a real-world AUV dataset, demonstrating how ambiguous acoustic correlator measurements are directly incorporated into a non-Gaussian SLAM solution, while using dead reckon tethering to overcome short term computational requirements.