Fast Simultaneous Localization and Mapping Based on Iterative Extended Kalman Filter Proposal Distribution and Linear Optimization Resampling

The location estimated accuracy of Autonomous Underwater Vehicle (AUV) and landmarks decrease because of the degeneracy and impoverishment of samples in standard Fast Simultaneous Localization And Mapping (FastSLAM) algorithm. A improved FastSLAM algorithm based on Iterative Extended Kalman Filter (IEKF) proposal distribution and linear optimization resampling is presented in order to solve this issue. The latest observation is integrated with IEKF in order to decrease the sample degeneracy while the new samples are produced by the linear combination of copied samples and some abandoned ones in order to reduce the sample impoverishment. The kinematic model of AUV, feature model and the measurement models of sensors are all established. And then features are extracted with Hough transform to build the global map. The experiment of the improved FastSLAM algorithm with trial data shows that it can avoid the degeneracy and impoverishment of samples effectively and enhance the location estimation accuracy of AUV and landmarks. Moreover, the consistency analysis showed that the method possesses the consistency of long term.