Added intial sonar code for alfred navigation