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

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.

[1]  Frank Chongwoo Park,et al.  Robot sensor calibration: solving AX=XB on the Euclidean group , 1994, IEEE Trans. Robotics Autom..

[2]  Mongi A. Abidi,et al.  Pose and motion estimation using dual quaternion-based extended Kalman filtering , 1998, Electronic Imaging.

[3]  Paul J. Besl,et al.  A Method for Registration of 3-D Shapes , 1992, IEEE Trans. Pattern Anal. Mach. Intell..

[4]  Gary K. L. Tam,et al.  Registration of 3D Point Clouds and Meshes: A Survey from Rigid to Nonrigid , 2013, IEEE Transactions on Visualization and Computer Graphics.

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

[6]  Nick Barnes,et al.  Rotation Averaging with Application to Camera-Rig Calibration , 2009, ACCV.

[7]  Howie Choset,et al.  Using Lie algebra for shape estimation of medical snake robots , 2014, 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[8]  F. L. Markley,et al.  Quaternion normalization in additive EKF for spacecraft attitude determination. [Extended Kalman Filters , 1991 .

[9]  Shiuh-Ku Weng,et al.  Video object tracking using adaptive Kalman filter , 2006, J. Vis. Commun. Image Represent..

[10]  Purang Abolmaesumi,et al.  Point-Based Rigid-Body Registration Using an Unscented Kalman Filter , 2007, IEEE Transactions on Medical Imaging.

[11]  T. Başar,et al.  A New Approach to Linear Filtering and Prediction Problems , 2001 .

[12]  Robert B. McGhee,et al.  An extended Kalman filter for quaternion-based orientation estimation using MARG sensors , 2001, Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems. Expanding the Societal Role of Robotics in the the Next Millennium (Cat. No.01CH37180).

[13]  Sven Molkenstruck,et al.  Low-Cost Laser Range Scanner and Fast Surface Registration Approach , 2006, DAGM-Symposium.

[14]  I. Bar-Itzhack,et al.  Novel quaternion Kalman filter , 2002, IEEE Transactions on Aerospace and Electronic Systems.

[15]  Darius Burschka,et al.  Spatio-temporal initialization for IMU to camera registration , 2011, 2011 IEEE International Conference on Robotics and Biomimetics.

[16]  Fadi Dornaika,et al.  Hand-Eye Calibration , 1995, Int. J. Robotics Res..

[17]  A. Jazwinski.  Stochastic Processes and Filtering Theory , 1970 .

[18]  Uwe D. Hanebeck,et al.  Recursive Bayesian calibration of depth sensors with non-overlapping views , 2012, 2012 15th International Conference on Information Fusion.

[19]  Malcolm D. Shuster,et al.  The Quaternion in Kalman Filtering , 2005 .

[20]  Joris De Schutter,et al.  Nonlinear Kalman Filtering for Force-Controlled Robot Tasks , 2010, Springer Tracts in Advanced Robotics.

[21]  Berthold K. P. Horn,et al.  Closed-form solution of absolute orientation using unit quaternions , 1987 .

[22]  Gregory S. Chirikjian,et al.  Sensor calibration with unknown correspondence: Solving AX=XB using Euclidean-group invariants , 2013, 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[23]  J. S. Goddarda,et al.  Pose and Motion Estimation Using Dual Quaternion-Based Extended Kalman Filtering , 2008 .

[24]  Ben Kenwright,et al.  A Beginners Guide to Dual-Quaternions: What They Are, How They Work, and How to Use Them for 3D Character Hierarchies , 2012, WSCG 2012.

[25]  Paul J. Besl,et al.  Method for registration of 3-D shapes , 1992, Other Conferences.

[26]  Jr. J.J. LaViola,et al.  A comparison of unscented and extended Kalman filtering for estimating quaternion motion , 2003, Proceedings of the 2003 American Control Conference, 2003..

[27]  J. Cremona,et al.  Proceedings of the London Mathematical Society , 1893 .

[28]  Clifford,et al.  Preliminary Sketch of Biquaternions , 1871 .

[29]  Yiu Cheung Shiu,et al.  Calibration of wrist-mounted robotic sensors by solving homogeneous transform equations of the form AX=XB , 1989, IEEE Trans. Robotics Autom..

[30]  Marc Levoy,et al.  Efficient variants of the ICP algorithm , 2001, Proceedings Third International Conference on 3-D Digital Imaging and Modeling.

[31]  H.H. Chen,et al.  A screw motion approach to uniqueness analysis of head-eye geometry , 1991, Proceedings. 1991 IEEE Computer Society Conference on Computer Vision and Pattern Recognition.

[32]  Xavier Pennec,et al.  A Framework for Uncertainty and Validation of 3-D Registration Methods Based on Points and Frames , 2004, International Journal of Computer Vision.

[33]  Fadi Dornaika,et al.  Simultaneous robot-world and hand-eye calibration , 1998, IEEE Trans. Robotics Autom..

[34]  Davide Spinello,et al.  Nonlinear Estimation With State-Dependent Gaussian Observation Noise , 2010, IEEE Transactions on Automatic Control.

[35]  Arthur C. Sanderson,et al.  A general representation for orientational uncertainty using random unit quaternions , 1994, Proceedings of the 1994 IEEE International Conference on Robotics and Automation.