Unscented information filter based multi-sensor data fusion using stereo camera, laser range finder and GPS receiver for vehicle localization

An Unscented Information Filter (UIF) based multi-sensor fusion method for ground vehicles localization in urban environments is presented and evaluated. UIF is used to fuse information acquired from a stereo vision system, a laser range finder and a GPS receiver in order to provide more robust vehicle localization results. Stereovision based visual odometry is used as input of the UIF prediction stage. By the analysis of stereo images, the relative motion is estimated after removing the tracking outliers. Then, laser range finder based positions and GPS readouts are used as measurements to update the information state vector after consistency validation. Besides, a MICP (modified iterative closest points) method is proposed to ameliorate the performance of laser scan alignment by applying dynamic distance error threshold. The obtained results demonstrate that the MICP method reduces the matching ambiguities of the classic ICP method. The proposed multi-sensor localization method was tested with real data and was evaluated using RTK-GPS as the ground truth. It shows that the UIF based method provides accurate localization services during GPS outages (e.g., GPS jumps, GPS mask), while avoiding the trajectory drift of stereo camera and laser range finder based methods.