Cooperative Localization of Vehicles in Three-Dimensional Space

Effective trajectory planning and cooperative control of multi-agent systems require accurate localization of the agents to perform collaborative missions. Accurate localization may be achieved by Global Positioning System (GPS) and simultaneous localization and mapping. However, GPS signals and fixed features may not be readily available, particularly in remote and unstructured environments. Under these circumstances, Cooperative Localization (CL) has been proposed as a short-term solution that can significantly improve vehicle pose estimation. CL algorithms have been developed and tested mainly on mobile robots and planar vehicles due to complexities of three-dimensional (3D) motion. In this paper, we present a CL algorithm for multi-agent systems comprised of 3D vehicles. Each vehicle’s pose is represented by three position and three orientation variables. Quaternions are employed to represent orientation and avoid singularities associated with Euler angle. Vehicle kinematic velocity relations are used to model vehicle dynamics with respect to a fixed reference frame. It is assumed a vehicle can take relative pose measurements of other neighboring vehicles within an ad hoc network of agents. We then designate the observed as target vehicles and the observers as base vehicles and determine the linearized output matrix of the relative measurements for Extended Kalman Filter (EKF) application. Simulations are presented to discuss the advantages and shortcomings of the algorithm.