Pose Estimation using Dual Quaternions and Moving Horizon Estimation

Abstract

This paper presents a moving horizon estimator (MHE) for estimating pose (attitude and position) of a dynamic system where pose measurements are available in the form of unit dual quaternions. A unit dual quaternion is an 8 parameter nonsingular representation of pose and has previously been used for pose estimation with Kalman filters (KF). We formulate a cost function in terms of the quaternion product and propose a MHE that includes the N latest measurements in the estimation. In addition, we suggest a measurement relation based on the Cayley transform of the noise, where the noise has a Gaussian distribution about the x-y-z and roll-pitch-yaw parameters of the pose. The MHE is compared against the dual quaternion multiplicative extended KF (DQ-MEKF) and the twistor-based unscented KF (T-UKF) through 100 Monte Carlo simulations, where the simulated data is generated according to the defined system dynamics. It is found that the MHE gives more accurate pose estimation results.