Pose-based GraphSLAM algorithm for robotic fish with a mechanical scanning sonar

This paper proposes a pose-based GraphSLAM algorithm for robotic fish equipped with a Mechanical Scanning Sonar (MSS) that has a low frequency of range readings. The main contribution of this paper is the construction of a pose graph as the front-end part of the normal GraphSLAM algorithm. The proposed algorithm has three stages as follows: 1) scan generation which incorporates a novel Extended Kalman Filter (EKF) based algorithm that takes the fish motion into account; 2) data association which is based on Mahanalobis distance and shape matching for determining loop closures; 3) scan matching which is for constraints calculation and pose graph construction. The constructed pose graph is then fed into a back-end optimizer - g2o for finding the optimal position of robotic fish. The viability and the accuracy of the proposed algorithm are verified by extensive simulations, compared with the dead reckoning and scan matching approaches.