An optimization based Moving Horizon Estimation with application to localization of Autonomous Underwater Vehicles

Localizing small Autonomous Underwater Vehicles (AUVs) that have limited payload and perception capability is of importance to promote popularization of underwater applications. Two different methodologies, filter and optimization based methods, can both be used to address the localization problem. But they are seldom rigorously compared and their relative advantages are rarely established. This paper presents a rigorous investigation on the relationship between these two methods. Based on this examination, a novel cooperative localization algorithm for the scenario where AUVs are localized by using range measurements from a single surface mobile beacon is proposed. The main contribution of this paper is threefold. First, major difference and close connection between filter based method and optimization based Maximum a Posteriori method are explicitly clarified by analytically solving optimization problems. Second, a novel localization algorithm combining a filter based extended Kalman filter and an optimization based Moving Horizon Estimation is developed for three-dimensional underwater localization in real-time and long-term applications. The algorithm allows data fusion of multiple sensors, imposes physical constraints on states and noises, bounds computational complexity, and achieves a compromise between better accuracy and lower computational requirement. Third, observability analysis of single beacon based localization algorithm is conducted in the context of nonlinear discrete time systems and a sufficient condition is derived. The observability and improved localization accuracy of the proposed localization algorithm are verified in a customized underwater simulator by extensive numerical simulations.