A Multihypotheses Importance Density for SLAM in Cluttered Scenarios

One of the most fundamental problems in simultaneous localization and mapping (SLAM) is the ability to take into account data association (DA) uncertainties. In this article, this problem is addressed by proposing a multihypotheses sampling distribution for particle filtering-based SLAM algorithms. By modeling the measurements and landmarks as random finite sets, an importance density approximation that incorporates DA uncertainties is derived. Then, a tractable Gaussian mixture model approximation of the multihypotheses importance density is proposed, in which each mixture component represents a different DA. Finally, an iterative method for approximating the mixture components of the sampling distribution is utilized and a partitioned update strategy is developed. Using synthetic and experimental data, it is demonstrated that the proposed importance density improves the accuracy and robustness of landmark-based SLAM in cluttered scenarios over state-of-the-art methods. At the same time, the partitioned update strategy makes it possible to include multiple DA hypotheses in the importance density approximation, leading to a favorable linear complexity scaling, in terms of the number of landmarks in the field-of-view.