Application and Assessment of Cooperative Localization in Three-Dimensional Vehicle Networks

The trajectory planning and control of multi-agent systems requires accurate localization, which may not be possible when GPS signals and fixed features required for SLAM are not available. Cooperative Localization (CL) in multi-agent systems offers a short-term solution that may significantly improve vehicle pose estimation. CL algorithms have been mainly developed and assessed for planar mobile robot networks due to complexities and singularities in three-dimensional (3D) motion. In this paper, we develop the required singularity-free equations and apply and assess an EKF-based CL for 3D vehicle networks. We assess the performance of CL with respect to the number of simultaneous and redundant measurements. We further assess CL performance with only relative position measurements available. Finally, experiments are performed to validate the proposed algorithms. We further investigate the effect of absolute position measurements in CL. Conclusions: Cooperative localization is an effective method when applied to 3D vehicle networks. However, CL does not improve localization with only relative position measurements, and thus previously reported results for 2D vehicle models were only effective due to relative orientation measurements. Absolute measurement reduces the overall localization errors much more significantly when there has been CL with prior relative position measurements.