Consistent Fusion of Correlated Pose Estimates on Matrix Lie Groups

Abstract

Pose fusion on the Special Euclidean groups ( <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$SE(n)$</tex-math></inline-formula> ) plays a key role in the localization of ground/aerial vehicles. However, for consistent fusion of pose estimates, the enduring correlation problem due to the observation of a common noise-corrupted process is yet to be addressed. We develop a methodology for the consistent fusion of correlated pose estimates that optimizes a quadratic cost function encompassing both self- and cross-correlation of local pose estimates. The error terms are calculated relative to a reference estimate and approximated using the Baker-Campbell-Hausdorff formula to obtain a non-iterative solution on the Lie algebra. The dependency of the fusion on the cross-covariance matrices is addressed via explicitly computing them through a recursive propagation of estimation errors at local extended Kalman filters. The efficacy of the proposed methodology is demonstrated by several numerical experiments conducted to (i) rigorously investigate the effect of correlation degree between local estimates on <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$SE(3)$</tex-math></inline-formula> and (ii) solve the localization problem of a rover on <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$SE(2)$</tex-math></inline-formula> with available pseudo pose measurements.