Adaptive Cubature Split Covariance Intersection Filter for Multi-Vehicle Cooperative Localization

Cooperative localization is still an open problem for autonomous vehicle driving tasks. Generally, there are two essential issues to be handled for multi-vehicle cooperative localization: one is how to use extra information shared by neighbour vehicles to augment ego-vehicle localization; the other is how to handle data sources of various error types used for state estimation, such as nonlinearity, dynamic noises, and correlation which may exist simultaneously. Previous works usually evaluate performance under one of the issues at one time, or they do not perform well under some extreme situations with both of the two types of problems. This paper presents an accurate and robust cooperative localization system with a decentralized framework, which not only takes advantages of inter vehicle relative pose estimation via shared information, but also ensures the performance under data sources of various error types simultaneously. We adopt a state-of-the-art point cloud registration method to obtain the cooperative relative pose estimation using shared information. In addition, we use the adaptive cubature split covariance intersection filter (ACSCIF) to estimate the vehicle state. This algorithm can approximate nonlinearity and deal with dynamic measurement noise by a third-order term using cubature transformation and innovation-based adaptive estimation (IAE) theory, also considering correlations among estimated sources inevitably exists in the decentralized mechanism. Finally, a comparative study based on the CARLA simulation platform demonstrates the potential and advantage of the proposed multi-vehicle cooperative localization system using ACSCIF in terms of accuracy, robustness and efficiency.