Underwater slam for estructured environments using and imaging sonar