In this project we address the problem of navigation for underwater vehicles in harbor surveillance tasks. Maintaining accurate localization for the duration of a mission is important for a variety of tasks, such as planning the vehicle trajectory and ensuring coverage of the area to be inspected. While acoustic based localization systems can be deployed, they work best in open water, and are of limited use in cluttered environments such as harbors.
The approach instead only uses onboard sensors in a simultaneous localization and mapping setting. We extract dense features from a forward looking imaging sonar and apply pair-wise registration between sonar frames. The registrations are combined with onboard velocity,attitude and acceleration sensors to obtain an improved estimate of the vehicle trajectory. We show results from several experiments that demonstrate drift free navigation in various underwater environments.