A Generic Multi-Sensor Fusion Scheme For Localization Of Autonomous Platforms Using Moving Horizon Estimation

In this paper, a generic multi-sensor fusion framework is developed for the localization of intelligent vehicles and mobile robots. The localization framework is based on moving horizon estimation (MHE). Unlike the commonly used probabilistic filtering algorithms - for example, extended Kalman filter (EKF) and unscented Kalman filter (UKF) - MHE relies on solving successive least squares optimization problems over the innovation of multiple sensors' measurements and a specific estimation horizon. In this paper, we present an efficient and generic multi-sensor fusion scheme, based on MHE. The proposed multi-sensor fusion scheme is capable of operating with different sensors' rates, missing measurements, and outliers. Moreover, the proposed scheme is based on a multi-threading architecture to reduce its computational cost, making it more feasible for practical applications. The MHE fusion method is tested using simulated data as well as real experimental data sequences from an intelligent vehicle and a mobile robot combining measurements from different sensors to get accurate localization results. The performance of MHE is compared against that of UKF, where the MHE estimation results show superior performance.