Collaborative localization of vehicle formations based on ranges and bearings

We examine the problem of jointly determining the positions of multiple underwater vehicles based on a set of pairwise range and bearing measurements taken over time. This extends prior work on the so-called (static) collaborative localization paradigm where a hybrid approach was proposed for seamless instantaneous fusion (i.e., no time dependence) of range and bearing measurements. To incorporate time we add to the original convexified least-squares cost function a regularizing term that penalizes deviations between predicted and computed vehicle positions at a given instant. The method operates progressively over time, with past estimates used for prediction at the current instant assuming a very simple quasilinear motion model. The method is amenable to parallelization, with simple gradient-like updates. Numerical results demonstrate promising accuracy gains (reduction on the order of 10 % in terms of root-mean-square positioning error) in simulations inspired by an underwater geoacoustic surveying application.