Multimodal data fusion using time-of-flight for simultaneous localization and mapping

In the proposed system, a simultaneous localization and mapping (SLAM) algorithm is performed using an extended Kalman filter (EKF), that allows to fuse information between the available sensors and leads to a reconstruction of the vehicle trajectory with the correct scale factor, as well as the sparse reconstruction of the surrounding environment. The sensors used to perform this task are a camera, a time-of-flight (TOF) sensor and an inertial measurement unit (IMU). To this end, the EKF has been suitably modified by adding intermediate prediction and correction steps between consecutive frames thanks to the higher rate of IMU measurements as well as by exploiting TOF measurements to initialize landmarks. The main aim of the proposed system is to show how the use of the TOF sensor allows the filter to estimate the trajectory of a moving robot also under adverse working conditions, such as the low frame rate of the camera, low-textured scenarios, low resolution and reliability of the TOF sensor and high noise of the accelerometer.