Robust range-only beacon localization

In this paper, we present a system capable of simultaneously estimating the position of an autonomous underwater vehicle (AUV) and the positions of stationary range-only beacons. Notably, our system does not require beacon positions a priori, and our system performs well even when range measurements are severely degraded by noise and outliers. We present a powerful outlier rejection method that can identify groups of range measurements that are consistent with each other, and a method for initializing beacon positions in an extended Kalman filter (EKF). We have successfully applied our algorithms to real-world data and have demonstrated a simultaneous localization and mapping (SLAM) system whose navigation performance is comparable to that of systems that assume known beacon locations.