Collaborative Multi-vehicle SLAM with moving object tracking

Although simultaneous localization and mapping (SLAM) algorithms are widely appreciated in mobile robot navigation, they can be further improved to suit practical applications in dynamic environmental conditions. One such important improvement is the detection and tracking of moving objects present in the sensor field of view (FOV). In this paper we propose to extend our recently introduced Collaborative Multi-vehicle SLAM (CMSLAM) solution based on the random finite set (RFS) representation of the feature map and measurements, by tracking both static and dynamic features. We represent static features observed during the SLAMprocess, along with dynamic features present in the current sensor FOV, as an augmented RFS. The corresponding probability density is propagated using a Bayes recursion, from which the static feature map and the estimates of dynamic feature locations can be obtained. Measurement update in the CMSLAM process is carried out only using the static feature map to take advantage of obvious accuracy improvements.