Joint Localization Based on Split Covariance Intersection on the Lie Group

This article presents a pose fusion method that accounts for the possible correlations among measurements. The proposed method can handle data fusion problems whose uncertainty has both independent and dependent parts. Different from the existing methods, the uncertainties of the various states or measurements are modeled on the Lie algebra and projected to the manifold through the exponential map, which is more precise than that modeled in the vector space. The correlation is based on the theory of covariance intersection, where the independent and dependent parts are split to yield a more consistent result. In this article, we provide a novel method for the correlated pose fusion algorithm on the manifold. Theoretical derivation and analysis are detailed first, and then, the experimental results are presented to support the proposed theory. The main contributions are threefold: First, we provide a theoretical foundation for the split covariance intersection filter performed on the manifold, where the uncertainty is associated with the Lie algebra. Second, the proposed method gives an explicit fusion formalism on $ \text{SE}(3)$ and $ \text{SE}(2)$, which covers the most use cases in the field of robotics. Third, we present a localization framework that can work for both single-robot and multirobot systems, where not only the fusion with possible correlation is derived on the manifold but also the state evolution and relative pose computation are performed on the manifold. The experimental results validate the advantage of this approach over state-of-the-art methods.