A Random-Finite-Set Approach to Bayesian SLAM

This paper proposes an integrated Bayesian frame work for feature-based simultaneous localization and map building (SLAM) in the general case of uncertain feature number and data association. By modeling the measurements and feature map as random finite sets (RFSs), a formulation of the feature-based SLAM problem is presented that jointly estimates the number and location of the features, as well as the vehicle trajectory. More concisely, the joint posterior distribution of the set-valued map and vehicle trajectory is propagated forward in time as measurements arrive, thereby incorporating both data association and feature management into a single recursion. Furthermore, the Bayes optimality of the proposed approach is established. A first-order solution, which is coined as the probability hypothesis density (PHD) SLAM filter, is derived, which jointly propagates the posterior PHD of the map and the posterior distribution of the vehicle trajectory. A Rao-Blackwellized (RB) 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 and experimental results demonstrate the merits of the proposed approach, particularly in situations of high clutter and data association ambiguity.