Unscented Dual Quaternion Particle Filter for SE(3) Estimation

We present a novel dual quaternion filter for recursive estimation of rigid body motions. Based on the sequential Monte Carlo scheme, particles are deployed on the manifold of unit dual quaternions. This allows non-parametric modeling of arbitrary distributions underlying on the SE(3) group. The proposal distribution for importance sampling is estimated particle-wise by a novel dual quaternion unscented Kalman filter (DQ-UKF). It is adapted to the manifold geometric structure and drives the prior particles towards high-likelihood regions on the manifold. The resultant unscented dual quaternion particle filter (U-DQPF) incorporates the most recently observed evidence, raising the particle efficiency considerably for nonlinear pose estimation tasks. Compared with ordinary particle filters and other parametric model-based dual quaternion filtering schemes, the proposed U-DQPF shows superior performance in nonlinear SE(3) estimation.

[1]  Howie Choset,et al.  Probabilistic pose estimation using a Bingham distribution-based linear filter , 2018, Int. J. Robotics Res..

[2]  Nando de Freitas,et al.  The Unscented Particle Filter , 2000, NIPS.

[3]  Gary R. Bradski,et al.  Monte Carlo Pose Estimation with Quaternion Kernels and the Bingham Distribution , 2011, Robotics: Science and Systems.

[4]  Hashim A Hashim.  Nonlinear Stochastic Estimators on the Special Euclidean Group SE(3) using Uncertain IMU and Vision Measurements , 2020, ArXiv.

[5]  U. Hanebeck,et al.  LRKF Revisited: The Smart Sampling Kalman Filter (S2KF) , 2015 .

[6]  Uwe D. Hanebeck,et al.  Stereo Visual SLAM Based on Unscented Dual Quaternion Filtering , 2019, 2019 22th International Conference on Information Fusion (FUSION).

[7]  Gerhard Kurz,et al.  Unscented Orientation Estimation Based on the Bingham Distribution , 2013, IEEE Transactions on Automatic Control.

[8]  Howie Choset,et al.  Estimating SE(3) elements using a dual quaternion based linear Kalman filter , 2016, Robotics: Science and Systems.

[9]  Wendelin Feiten,et al.  6D Pose Uncertainty in Robotic Perception , 2009 .

[10]  Neil J. Gordon,et al.  A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking , 2002, IEEE Trans. Signal Process..

[11]  Uwe D. Hanebeck,et al.  Grid-Based Quaternion Filter for SO(3) Estimation , 2020, 2020 European Control Conference (ECC).

[12]  Gerhard Kurz,et al.  A new probability distribution for simultaneous representation of uncertain position and orientation , 2014, 17th International Conference on Information Fusion (FUSION).

[13]  Søren Hauberg,et al.  Unscented Kalman Filtering on Riemannian Manifolds , 2013, Journal of Mathematical Imaging and Vision.

[14]  Gerhard Kurz,et al.  Application of Discrete Recursive Bayesian Estimation on Intervals and the Unit Circle to Filtering on SE(2) , 2018, IEEE Transactions on Industrial Informatics.

[15]  E. Kraft,et al.  A quaternion-based unscented Kalman filter for orientation tracking , 2003, Sixth International Conference of Information Fusion, 2003. Proceedings of the.

[16]  Wendelin Feiten,et al.  MPG - A Framework for Reasoning on 6 DOF Pose Uncertainty , 2017, ArXiv.

[17]  Hashim A. Hashim,et al.  Nonlinear Pose Filters on the Special Euclidean Group SE(3) With Guaranteed Transient and Steady-State Performance , 2019, IEEE Transactions on Systems, Man, and Cybernetics: Systems.

[18]  Jérôme Malick,et al.  Projection-like Retractions on Matrix Manifolds , 2012, SIAM J. Optim..

[19]  Nassir Navab,et al.  Camera Pose Filtering with Local Regression Geodesics on the Riemannian Manifold of Dual Quaternions , 2017, 2017 IEEE International Conference on Computer Vision Workshops (ICCVW).

[20]  Mark L. Psiaki,et al.  Estimation using quaternion probability densities on the unit hypersphere , 2006 .

[21]  Leslie Pack Kaelbling,et al.  Tracking the spin on a ping pong ball with the quaternion Bingham filter , 2014, 2014 IEEE International Conference on Robotics and Automation (ICRA).

[22]  Jeffrey K. Uhlmann,et al.  Unscented filtering and nonlinear estimation , 2004, Proceedings of the IEEE.

[23]  Daniel Choukroun,et al.  Vision-aided Spacecraft Relative Pose Estimation via Dual Quaternions , 2019, 2019 IEEE 58th Conference on Decision and Control (CDC).

[24]  Uwe D. Hanebeck,et al.  Hyperspherical Deterministic Sampling Based on Riemannian Geometry for Improved Nonlinear Bingham Filtering , 2019, 2019 22th International Conference on Information Fusion (FUSION).

[25]  Hashim A. Hashim,et al.  Nonlinear Stochastic Position and Attitude Filter on the Special Euclidean Group 3 , 2018, J. Frankl. Inst..

[26]  Uwe D. Hanebeck,et al.  Geometry-Driven Stochastic Modeling of SE(3) States Based on Dual Quaternion Representation , 2019, 2019 IEEE International Conference on Industrial Cyber Physical Systems (ICPS).

[27]  Daniel Cremers,et al.  Camera-based navigation of a low-cost quadrocopter , 2012, 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems.