A Belief Propagation Algorithm for Multipath-based SLAM with Multiple Map Features: A mmWave MIMO Application

In this paper, we present a multipath-based simultaneous localization and mapping (SLAM) algorithm that continuously adapts mulitiple map feature (MF) models describing specularly reflected multipath components (MPCs) from flat surfaces and point-scattered MPCs, respectively. We develop a Bayesian model for sequential detection and estimation of interacting MF model parameters, MF states and mobile agent's state including position and orientation. The Bayesian model is represented by a factor graph enabling the use of belief propagation (BP) for efficient computation of the marginal posterior distributions. The algorithm also exploits amplitude information enabling reliable detection of weak MFs associated with MPCs of very low signal-to-noise ratios (SNRs). The performance of the proposed algorithm is evaluated using real millimeter-wave (mmWave) multiple-input-multiple-output (MIMO) measurements with single base station setup. Results demonstrate the excellent localization and mapping performance of the proposed algorithm in challenging dynamic outdoor scenarios.