Unified Cooperative Localization via Augmented Factor Graph and Error State Message Passing

cooperative localization (CL) is a promising approach to improve the localization performance. However, many existing CL methods inadequately utilize the correlations within multiagent systems and underperform in scenarios involving nonlinear system model. To address this issue, this work proposes an unified CL (UCL) estimator for both cooperative self-localization (CSL) and cooperative relative-localization (CRL) processes. By incorporating an additional pseudo-CRL process, a consensus strategy and relative motion constraints, a novel augmented factor graph (FG) framework is designed to fully exploit the potential constraints in a multiagent system. Additionally, a novel error state message passing (ES-MP) scheme in error state domain is employed to improve the validity of linearization process when dealing with nonlinear system models, thereby further improving the estimation accuracy. The simulation and experimental results demonstrate that UCL outperforms many existing CL methods in both CSL and CRL accuracy. Moreover, UCL achieves a better performance compared to particle sampling-based methods with significant lower computational load, making it a computationally efficient choice for CL systems.