Bathymetric Particle Filter SLAM With Graph-Based Trajectory Update Method

A graph-based particle filter bathymetric simultaneous localization and mapping (BSLAM) method is proposed to solve the oscillation problem of the trajectories estimated by particles when using a low precise vehicle motion model and obtain accurate navigation results for autonomous underwater vehicles (AUVs). A graph-based trajectory update method is proposed to update the trajectories stored in particles before particle weighting to weaken the influence of the low precise odometer model on the particle trajectories. A particle weighting method based on submap matching is proposed to improve the robustness of the particle filter. Besides, a graph-based map generation method is proposed to solve the map selection problem of the particle filtering theory. The performance of the proposed method is demonstrated using a simulated dataset and a field dataset collected from a sea trial. The results show that the proposed method is more accurate and effective compared with a state-of-art particle filter BSLAM method.