Modifications to fix buoy since stephens simulator didn't work
Start of sonar manipulation code
config file support for visual servoing
some fixes to utility code