Underwater Localization and Mapping with Imaging Sonar