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

In this article, an SE(3)-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 SO(3)-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 SE(3) estimator from the literature and a Monte Carlo simulation is carried out to provide credence to the accuracy of the proposed filter.