Pose Estimation based on a Dual Quaternion Feedback Particle Filter

Abstract

Fast and accurate pose estimation is essential for many robotic applications such as SLAM, manipulation, and 3D point registration. Existing solutions to this problem suffer from either high computation overhead due to the nonlinear features or accuracy loss due to linear approximation. In this paper, we propose a dual quaternion feedback particle filter (DQFPF) that can capture the nonlinear factors in the observation model and use the optimal control theory to estimate the pose. To avoid particle degeneracy caused by sequential importance sampling and resampling, we present a feedback particle update formula to speed up the optimization with fewer particles being sampled. Simulation results show that in known corresponding cases our approach can converge to the correct pose more efficiently than the state-of-the-art. A similar conclusion can also be drawn in real applications of unknown corresponding cases, i.e., point cloud stitching and visual odometry estimation.