Stochastic Cloning and Smoothing for Fusion of Multiple Relative and Absolute Measurements for Localization and Mapping

A mobile robot is reliant on precise and robust localization and mapping for autonomous navigation. For this purpose, sensor fusion techniques are employed to combine measurements of multiple sensor data sources. The well-known Extended Kalman filter is the standard approach to integrate absolute measurements; however, multiple relative measurements, i.e., measured differences between the current system state and a past system state, cannot be directly incorporated into the filter. This paper presents a fusion algorithm for the integration of absolute and multiple relative measurements for localization and mapping of mobile robots. A novel approach exploiting concurrent stochastic cloning and smoothing is introduced for robust inclusion of additional relative measurements. The proposed fusion method is applied to perform simultaneous localization and mapping with sensor data from an IMU, a GPS, wheel odometry, and scan matching of data from a 3D LiDAR.