Sonar and video data fusion for robot localization and environment feature estimation