The Dynamic Bearing Observability Matrix Nonlinear Observability and Estimation for Multi-Agent Systems

We consider the problem of localization in multiagent formations with bearing only measurements, and analyze the fundamental observability properties for dynamic agents. The current well-established approach is based on the socalled rigidity matrix, and its algebraic properties (e.g., its rank and nullspace). This method is typically motivated using first-order derivatives, and shows, among other facts, that the global scale of the formation is not observable. This work shows that current results represent an incomplete view of the problem. In particular, we show that 1) current methods are a particular instantiation of nonlinear observability theory, 2) we can introduce the concept of the dynamic bearing observability matrix from higher order derivatives to study the observability of dynamic formations, and 3) the global scale is, in fact, generally observable when the agents move according to known inputs. We use tools from Riemannian geometry and Lie group theory to tackle, in a general and principled way, the general formulation of the localization problem with states that include both rotations and translations. Finally, we verify our theoretical results by deriving and applying, in both simulations and real experiments on UAVs, a centralized Extended Kalman Filter on Lie groups that is able to estimate the global scale of a moving formation.