SLAM with single cluster PHD filters

Recent work by Mullane, Vo, and Adams has re-examined the probabilistic foundations of feature-based Simultaneous Localization and Mapping (SLAM), casting the problem in terms of filtering with random finite sets. Algorithms were developed based on Probability Hypothesis Density (PHD) filtering techniques that provided superior performance to leading feature-based SLAM algorithms in challenging measurement scenarios with high false alarm rates, high missed detection rates, and high levels of measurement noise. We investigate this approach further by considering a hierarchical point process, or single-cluster multi-object, model, where we consider the state to consist of a map of landmarks conditioned on a vehicle state. Using Finite Set Statistics, we are able to find tractable formulae to approximate the joint vehicle-landmark state based on a single Poisson multi-object assumption on the predicted density. We describe the single-cluster PHD filter and the practical implementation developed based on a particle-system representation of the vehicle state and a Gaussian mixture approximation of the map for each particle. Synthetic simulation results are presented to compare the novel algorithm against the previous PHD filter SLAM algorithm. Results presented indicate a superior performance in vehicle and map landmark localization, and comparable performance in landmark cardinality estimation.