Grid-based localization and local mapping with moving object detection and tracking

We present a real-time algorithm for simultaneous localization and local mapping (local SLAM) with detection and tracking of moving objects (DATMO) in dynamic outdoor environments from a moving vehicle equipped with a laser scanner, short-range radars and odometry. To correct the vehicle odometry we introduce a new fast implementation of incremental scan matching method that can work reliably in dynamic outdoor environments. After obtaining a good vehicle localization, the map surrounding of the vehicle is updated incrementally and moving objects are detected without a priori knowledge of the targets. Detected moving objects are finally tracked by a Multiple Hypothesis Tracker (MHT) coupled with an adaptive Interacting Multiple Model (IMM) filter. The experimental results on datasets collected from different scenarios such as: urban streets, country roads and highways demonstrate the efficiency of the proposed algorithm.