RBP-OP: Distributed Robust Belief Propagation Method with Odometry Preintegration for Multirobot Collaborative Localization

Distributed cooperative localization is essential to operate successfully for multirobot systems, especially in scenarios where absolute information cannot be continuously obtained. With the properties of distributed processing and message passing, Gaussian belief propagation has proven to be effective for achieving accurate pose estimation, making it a promising method for future distributed cooperative localization. Unfortunately, existing Gaussian belief propagation-based distributed cooperative localization methods are sensitive to measurement outliers and measurement nonlinearity, leading to poor performance in such environments. To address these issues, a novel distributed robust belief propagation method with odometry preintegration (RBP-OP) is proposed to mitigate the effects of measurement outliers and measurement nonlinearity. Firstly, the belief of variable node is modeled as the Gaussian distribution and the probability density function of external measurement factor node is modeled as the Student's t-distribution. The message between external measurement factor node and variable node is computed by modifying the measurement noise covariance matrix adaptively, which significantly reduces the impact of outliers. Secondly, a novel wheel-speed odometry factor is derived based on the preintegration method, which enables forward-backward iteration, and then mitigates the effects of measurement nonlinearity. Finally, extensive simulations and experiments show that the proposed RBP-OP method offers superior filtering robustness and estimation accuracy compared to the existing state-of-the-art methods.