Cooperative Localization Using the 3D Euler–Lagrange Vehicle Model

Abstract

Kalman filter-based cooperative localization (CL) algorithms have been shown to significantly improve pose estimations within networks of vehicles but have relied predominantly on two-dimensional kinematic models of the member agents. An inherent deficiency of the commonly employed kinematic vehicle model is the ineffectiveness of CL with only relative position measurements. In this work, we present a singularity-free CL using the full three-dimensional (3D) nonlinear dynamic vehicle model suitable for decentralized control and navigation of heterogeneous networks. We develop the algorithm, present Monte Carlo simulation results with relative pose measurements, and assess the algorithm performance as the number of measurements increases. We further demonstrate that CL with only relative position measurements is effective when using the dynamic model and benefits from increasing number of measurements. We also evaluate the performance of CL with respect to measurement task distribution, which is important in cooperative control of autonomous vehicles.