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.