Feature tracking for underwater navigation using sonar

Tracking sonar features in real time on an underwater robot is a challenging task. One reason is the low observability of the sonar in some directions. For example, using a blazed array sonar one observes range and the angle to the array axis with fair precision. The angle around the axis is poorly constrained. This situation is problematic for tracking features in world frame Cartesian coordinates as the error surfaces will not be ellipsoids. Thus Gaussian tracking of the features will not work properly. The situation is similar to the problem of tracking features in camera images. There the unconstrained direction is depth and its errors are highly non-Gaussian. We propose a solution to the sonar problem that is analogous to the successful inverse depth feature parameterization for vision tracking, introduced by [1]. We parameterize the features by the robot pose where it was first seen and the range/bearing from that pose. Thus the 3D features have 9 parameters that specify their world coordinates. We use a nonlinear transformation on the poorly observed bearing angle to give a more accurate Gaussian approximation to the uncertainty. These features are tracked in a SLAM framework until there is enough information to initialize world frame Cartesian coordinates for them. The more compact representation can then be used for a global SLAM or localization purposes. We present results for a system running real time underwater SLAM/localization. These results show that the parameterization leads to greater consistency in the feature location estimates.