Joint Localization Based on Split Covariance Intersection on the Lie Group

Abstract

This paper 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 part and dependent part. 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 dealing of 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 paper, we provide a novel method for 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: (1) We provide a theoretical foundation for the split covariance intersection filter performed on the manifold, where the uncertainty is associated on the Lie algebra. (2) The proposed method gives an explicit fusion formalism on SE(3) and SE(2), which covers the most use cases in the field of robotics. (3) We present a localization framework that can work both for single robot and multi-robots systems, where not only the fusion with possible correlation is derived on the manifold, the state evolution and relative pose computation are also performed on the manifold. Experimental results validate its advantage over state-of-the-art methods.

A Survey of Vehicle Localization: Performance Analysis and Challenges

“…However, the CI method has pessimistic estimate results since the source data is treated as totally correlated, and the independent part is not considered. Therefore, the author of [138] proposed the split covariance intersection filter (SCIF), which can estimate the source data in both correlated and independent parts. Moreover, the author of [139] used the SCIF to fuse the data from GNSS, camera, LiDAR, and HD maps, which achieved an accuracy with RMSE of 0.27 m.…”

Sensors and Sensor Fusion Methodologies for Indoor Odometry: A Review

“…Variants of the EKF, such as the UKF [ 260 ], Multi-State Constraint Kalman Filter (MSCKF) [ 261 , 262 ]—which incorporates poses of past frames to marginalize features from the state space. Iterated EKF [ 263 ], Cubature Kalman Filter (CKF) [ 264 ], fuzzy logic KF [ 265 ], covariance intersection KF [ 266 ] and invariant EKF based on the Lie group [ 267 ], have been proposed in the literature.…”

Simultaneous Position and Orientation Estimation for Visible Light Systems With Multiple LEDs and Multiple PDs

“…5 A valid mapping R from the tangent space T M to the manifold M, i.e., R : T M → M, is called a retraction on a manifold M, which has to satisfy several particular properties (please refer to the Definition 3.41 in [45, p. 46] for the formal definition). For Lie groups, one valid retraction is the Lie exponential map given by the matrix exponential function [45, p. 152], which is intensively used in the robotic society [47,48,49].…”