SLAM With Dynamic Targets via Single-Cluster PHD Filtering

Abstract

This paper presents the first algorithm for simultaneous localization and mapping (SLAM) that can estimate the locations of both dynamic and static features in addition to the vehicle trajectory. We model the feature-based SLAM problem as a single-cluster process, where the vehicle motion defines the parent, and the map features define the daughter. Based on this assumption, we obtain tractable formulae that define a Bayesian filter recursion. The novelty in this filter is that it provides a robust multi-object likelihood which is easily understood in the context of our starting assumptions. We present a particle/Gaussian mixture implementation of the filter, taking into consideration the challenges that SLAM presents over target tracking with stationary sensors, such as changing fields of view and a mixture of static and dynamic map features. Monte Carlo simulation results are given which demonstrate the filter's effectiveness with high measurement clutter and non-linear vehicle motion