Unscented Kalman Filtering on Lie Groups for Fusion of IMU and Monocular Vision

Combining visual information with inertial measurements represents one popular approach to achieve robust and autonomous navigation in robotics, specifically for low-cost aerial vehicles in GPS-denied environments. In this paper, building upon both the recent theory of Unscented Kalman Filtering on Lie Groups (UKF-LG) and the theory of invariant Kalman filter based SLAM we proposed recently, an innovative UKF for the monocular visual inertial problem is derived, where the body pose, velocity, and the 3D landmarks' positions are viewed as a single element of a (high dimensional) Lie group SE2+p(3), and where accelerometers' and gyrometers' biases are appended to the state and estimated as well. The efficiency of the approach is validated on a real data set, where the proposed filter is shown to compare favorably to the conventional UKF, and to some recent UKF variants of the literature.

[1]  Frank Dellaert,et al.  On-Manifold Preintegration for Real-Time Visual--Inertial Odometry , 2015, IEEE Transactions on Robotics.

[2]  Axel Barrau,et al.  Invariant Kalman Filtering , 2018, Annu. Rev. Control. Robotics Auton. Syst..

[3]  Axel Barrau,et al.  The Invariant Extended Kalman Filter as a Stable Observer , 2014, IEEE Transactions on Automatic Control.

[4]  Paul Timothy Furgale,et al.  Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems , 2014, IEEE Transactions on Robotics.

[5]  Rudolph van der Merwe,et al.  The square-root unscented Kalman filter for state and parameter-estimation , 2001, 2001 IEEE International Conference on Acoustics, Speech, and Signal Processing. Proceedings (Cat. No.01CH37221).

[6]  Carlo Tomasi,et al.  Good features to track , 1994, 1994 Proceedings of IEEE Conference on Computer Vision and Pattern Recognition.

[7]  Jean-Philippe Condomines,et al.  Unscented Kalman filtering on Lie groups , 2017, 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[8]  Axel Barrau,et al.  An EKF-SLAM algorithm with consistency properties , 2015, ArXiv.

[9]  Timothy D. Barfoot,et al.  State Estimation for Robotics , 2017 .

[10]  Anastasios I. Mourikis,et al.  High-precision, consistent EKF-based visual-inertial odometry , 2013, Int. J. Robotics Res..

[11]  Vijay Kumar,et al.  Metrics and Connections for Rigid-Body Kinematics , 1999, Int. J. Robotics Res..

[12]  Kazuya Yoshida,et al.  Collaborative mapping of an earthquake‐damaged building via ground and aerial robots , 2012, J. Field Robotics.

[13]  Gregory S. Chirikjian,et al.  Error propagation on the Euclidean group with applications to manipulator kinematics , 2006, IEEE Transactions on Robotics.

[14]  Dimitrios G. Kottas,et al.  Observability-constrained Vision-aided Inertial Navigation , 2012 .

[15]  Silvere Bonnabel.  Symmetries in observer design: review of some recent results and applications to EKF-based SLAM , 2011, ArXiv.

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

[17]  Stergios I. Roumeliotis,et al.  A Quadratic-Complexity Observability-Constrained Unscented Kalman Filter for SLAM , 2013, IEEE Transactions on Robotics.

[18]  Audrey Giremus,et al.  Continuous-Discrete Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions , 2014, Journal of Mathematical Imaging and Vision.

[19]  J. M. M. Montiel,et al.  ORB-SLAM: A Versatile and Accurate Monocular SLAM System , 2015, IEEE Transactions on Robotics.

[20]  Juan D. Tardós,et al.  Visual-Inertial Monocular SLAM With Map Reuse , 2016, IEEE Robotics and Automation Letters.

[21]  Gamini Dissanayake,et al.  An invariant-EKF VINS algorithm for improving consistency , 2017, 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[22]  Flavio Fontana,et al.  Autonomous, Vision‐based Flight and Live Dense 3D Mapping with a Quadrotor Micro Aerial Vehicle , 2016, J. Field Robotics.

[23]  Vijay Kumar,et al.  Visual inertial odometry for quadrotors on SE(3) , 2016, 2016 IEEE International Conference on Robotics and Automation (ICRA).

[24]  Gamini Dissanayake,et al.  Convergence and Consistency Analysis for a 3-D Invariant-EKF SLAM , 2017, IEEE Robotics and Automation Letters.

[25]  G. Chirikjian.  Stochastic Models, Information Theory, and Lie Groups, Volume 2 , 2012 .

[26]  Frank Chongwoo Park,et al.  A Lie Group Formulation of Robot Dynamics , 1995, Int. J. Robotics Res..

[27]  Eduardo Mario Nebot,et al.  Consistency of the EKF-SLAM Algorithm , 2006, 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[28]  Simon J. Julier,et al.  The scaled unscented transformation , 2002, Proceedings of the 2002 American Control Conference (IEEE Cat. No.CH37301).

[29]  A. D. Lewis,et al.  Geometric control of mechanical systems : modeling, analysis, and design for simple mechanical control systems , 2005 .

[30]  David W. Murray,et al.  An O(N²) Square Root Unscented Kalman Filter for Visual Simultaneous Localization and Mapping , 2009, IEEE Transactions on Pattern Analysis and Machine Intelligence.

[31]  Jean Ponce,et al.  Computer Vision: A Modern Approach , 2002 .