SE(3)-Constrained Extended Kalman Filtering for Rigid Body Pose Estimation

Abstract

In this article, an <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> -constrained extended Kalman filter is proposed in continuous time as well as in a more practical continuous-discrete framework. The filter allows for the state estimation of the 6-DOF rigid body motion while accounting for measurement error statistics and using the rotation matrix instead of quaternions or other attitude parameterizations. The proposed filter differs from the recently proposed <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$SO(3)$</tex-math></inline-formula> -constrained attitude filter in that only a subset of the configuration states are constrained in the present filter. Its effectiveness is demonstrated in a numerical example in which its performance is compared with that of an existing <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> estimator from the literature and a Monte Carlo simulation is carried out to provide credence to the accuracy of the proposed filter.