A Random Set Formulation for Bayesian SLAM

This paper presents an alternative formulation for the Bayesian feature-based simultaneous localisation and mapping (SLAM) problem, using a random finite set approach. For a feature based map, SLAM requires the joint estimation of the vehicle location and the map. The map itself involves the joint estimation of both the number of features and their states (typically in a 2D Euclidean space), as an a priori unknown map is completely unknown in both landmark location and number. In most feature based SLAM algorithms, so-called 'feature management' algorithms as well as data association hypotheses along with extended Kalman filters are used to generate the joint posterior estimate. This paper, however, presents a recursive filtering algorithm which jointly propagates both the estimate of the number of landmarks, their corresponding states, and the vehicle pose state, without the need for explicit feature management and data association algorithms.Using a finite set-valued joint vehicle-map state and set-valued measurements, the first order statistic of the set, called the intensity, is propagated via the probability hypothesis density (PHD) filter, from which estimates of the map and vehicle can be jointly extracted. Assuming a mildly non-linear Gaussian system, an extended-Kalman Gaussian Mixture implementation of the recursion is then tested for both feature-based robotic mapping (known location) and SLAM. Results from the experiments show promising performance for the proposed SLAM framework, especially in environments of high spurious measurements.