AUV Single Beacon Range-Only SLAM with a SOG Filter

Abstract

This paper presents a solution to the Simultaneous Localization and Mapping (SLAM) problem of an AUV which uses range-only measurements towards a single beacon. It simultaneously estimates the beacon (a priori unknown) and AUV (a priori known) positions. Conventional EKF-SLAM methods cannot deal with this problem since the resultant probability density function (pdf) of the beacon-position is severely non-Gaussian during the initialization. Particle filters can do the job but, depending on the size of the space where the beacon lays, a significant number of particles may be required to populate the solution domain, making difficult to achieve real-time performance. In this paper we present a filter which represents the joint pdf of the robot and beacon positions as a Multivariate Gaussian Mixture which has the potential to represent an arbitrary pdf with a number of Gaussian 'particles' significantly smaller than the one that would be necessary if conventional particles were used instead. In our method, each particle contains a full EKF over vehicle position/velocity and the beacon position. The method is applied to locate a sub-sea panel and home to it autonomously in order to establish visual contact to later launch a visual servoing based docking task. The method is demonstrated through real-time Hardware-in-the-loop experiments (HIL) where the robot follows an observable trajectory.