Invariant Kalman Filtering for Visual Inertial SLAM

Combining visual information with inertial measurements is a popular approach to achieve robust and autonomous navigation in robotics, specifically in GPS-denied environments. In this paper, building upon both the recent theory of Unscented Kalman Filtering on Lie Groups (UKF-LG) and more generally the theory of invariant Kalman filtering (IEKF), an innovative UKF is derived for the monocular visual simultaneous localization and mapping (SLAM) problem. The body pose, velocity, and the 3D landmarks' positions are viewed as a single element of a (high dimensional) Lie group $SE_{2+p}(3)$, which constitutes the state, and where the accelerometers' and gyrometers' biases are appended to the state and estimated as well. The efficiency of the approach is validated both on simulations and on five real datasets.

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

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

[3]  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).

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

[5]  Daniel Cremers,et al.  LSD-SLAM: Large-Scale Direct Monocular SLAM , 2014, ECCV.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

[23]  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.

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

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

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

[27]  Chan Gook Park,et al.  Consistent EKF-Based Visual-Inertial Odometry on Matrix Lie Group , 2018, IEEE Sensors Journal.

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

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

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