An Ant–Based Filtering Random–Finite–Set Approach to Simultaneous Localization and Mapping

Inspired by ant foraging, as well as modeling of the feature map and measurements as random finite sets, a novel formulation in an ant colony framework is proposed to jointly estimate the map and the vehicle trajectory so as to solve a feature-based simultaneous localization and mapping (SLAM) problem. This so-called ant-PHD-SLAM algorithm allows decomposing the recursion for the joint map-trajectory posterior density into a jointly propagated posterior density of the vehicle trajectory and the posterior density of the feature map conditioned on the vehicle trajectory. More specifically, an ant-PHD filter is proposed to jointly estimate the number of map features and their locations, namely, using the powerful search ability and collective cooperation of ants to complete the PHD-SLAM filter time prediction and data update process. Meanwhile, a novel fast moving ant estimator (F-MAE) is utilized to estimate the maneuvering vehicle trajectory. Evaluation and comparison using several numerical examples show a performance improvement over recently reported approaches. Moreover, the experimental results based on the robot operation system (ROS) platform validate the consistency with the results obtained from numerical simulations.