Improving Underwater Navigation and Docking with Three-Dimensional Sonar