Rao-Blackwellised PHD SLAM

This paper proposes a tractable solution to feature-based (FB) SLAM in the presence of data association uncertainty and uncertainty in the number of features. By modeling the feature map as a random finite set (RFS), a rigorous Bayesian formulation of the FB-SLAM problem that accounts for uncertainty in the number of features and data association is presented. As such, the joint posterior distribution of the set-valued map and vehicle trajectory is propagated forward in time as measurements arrive. A first order solution, coined the PHD-SLAM filter, is derived, which jointly propagates the posterior PHD or intensity function of the map and the posterior distribution of the trajectory of the vehicle. A Rao-Blackwellised implementation of the PHD-SLAM filter is proposed based on the Gaussian mixture PHD filter for the map and a particle filter for the vehicle trajectory. Simulated results demonstrate the merits of the proposed approach, particularly in situations of high clutter and data association ambiguity.