Estimating SE(3) elements using a dual quaternion based linear Kalman filter

Abstract

Many applications in robotics such as registration, object tracking, sensor calibration, etc. use Kalman filters to estimate a time invariant SE(3) element by locally linearizing a non-linear measurement model. Linearization-based filters tend to suffer from inaccurate estimates, and in some cases divergence, in the presence of large initialization errors. In this work, we use a dual quaternion to represent the SE(3) element and use multiple measurements simultaneously to rewrite the measurement model in a truly linear form with state dependent measurement noise. Use of the linear measurement model bypasses the need for any linearization in prescribing the Kalman filter, resulting in accurate estimates while being less sensitive to initial estimation error. To show the broad applicability of this approach, we derive linear measurement models for applications that use either position measurements or pose measurements. A procedure to estimate the state dependent measurement uncertainty is also discussed. The efficacy of the formulation is illustrated using simulations and hardware experiments for two applications in robotics: rigid registration and sensor calibration.