An EKF-SLAM algorithm with consistency properties

In this paper we address the inconsistency of the EKF-based SLAM algorithm that stems from non-observability of the origin and orientation of the global reference frame. We prove on the non-linear two-dimensional problem with point landmarks observed that this type of inconsistency is remedied using the Invariant EKF, a recently introduced variant ot the EKF meant to account for the symmetries of the state space. Extensive Monte-Carlo runs illustrate the theoretical results.

[1]  Hugh F. Durrant-Whyte,et al.  Simultaneous localization and mapping: part I , 2006, IEEE Robotics & Automation Magazine.

[2]  J. Gauthier,et al.  Observability and observers for non-linear systems , 1986, 1986 25th IEEE Conference on Decision and Control.

[3]  Silvere Bonnabel,et al.  An invariant Linear Quadratic Gaussian controller for a simplified car , 2014, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[4]  Javier Ibanez Guzman,et al.  On the Observability and Observability Analysis of SLAM , 2006, 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[5]  Roland Siegwart,et al.  Some results on SLAM and the closing the loop problem , 2005, 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[6]  李幼升,et al.  Ph , 1989 .

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

[8]  Jeffrey K. Uhlmann,et al.  A counter example to the theory of simultaneous localization and map building , 2001, Proceedings 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat. No.01CH37164).

[9]  Gamini Dissanayake,et al.  Sparse Local Submap Joining Filter for Building Large-Scale Maps , 2008, IEEE Transactions on Robotics.

[10]  Juan Andrade-Cetto,et al.  The effects of partial observability when building fully correlated maps , 2005, IEEE Transactions on Robotics.

[11]  Carlos Silvestre,et al.  Simultaneous localization and mapping for aerial vehicles: a 3-D sensor-based GAS filter , 2016, Auton. Robots.

[12]  Axel Barrau,et al.  Intrinsic Filtering on Lie Groups With Applications to Attitude Estimation , 2013, IEEE Transactions on Automatic Control.

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

[14]  Sebastian Thrun,et al.  Probabilistic robotics , 2002, CACM.

[15]  Stergios I. Roumeliotis,et al.  An observability-constrained sliding window filter for SLAM , 2011, 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[16]  Silvere Bonnabel,et al.  Left-invariant extended Kalman filter and attitude estimation , 2007, 2007 46th IEEE Conference on Decision and Control.

[17]  Stergios I. Roumeliotis,et al.  Analysis and improvement of the consistency of extended Kalman filter based SLAM , 2008, 2008 IEEE International Conference on Robotics and Automation.

[18]  Hugh F. Durrant-Whyte,et al.  A solution to the simultaneous localization and map building (SLAM) problem , 2001, IEEE Trans. Robotics Autom..

[19]  José A. Castellanos,et al.  Robocentric map joining: Improving the consistency of EKF-SLAM , 2007, Robotics Auton. Syst..

[20]  Stergios I. Roumeliotis,et al.  Bayesian estimation and Kalman filtering: a unified framework for mobile robot localization , 2000, Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065).

[21]  Gamini Dissanayake,et al.  Convergence and Consistency Analysis for Extended Kalman Filter Based SLAM , 2007, IEEE Transactions on Robotics.

[22]  Hugh F. Durrant-Whyte,et al.  Simultaneous Localization and Mapping with Sparse Extended Information Filters , 2004, Int. J. Robotics Res..

[23]  Alan F. Lynch,et al.  Invariant Observer Design for a Helicopter UAV Aided Inertial Navigation System , 2013, IEEE Transactions on Control Systems Technology.

[24]  Carlos H. Muravchik,et al.  Posterior Cramer-Rao bounds for discrete-time nonlinear filtering , 1998, IEEE Trans. Signal Process..

[25]  François Goulette,et al.  Invariant EKF Design for Scan Matching-Aided Localization , 2015, IEEE Transactions on Control Systems Technology.

[26]  Thiagalingam Kirubarajan,et al.  Estimation with Applications to Tracking and Navigation , 2001 .

[27]  Robert F. Stengel,et al.  Optimal Control and Estimation , 1994 .

[28]  Stergios I. Roumeliotis,et al.  Observability-based Rules for Designing Consistent EKF SLAM Estimators , 2010, Int. J. Robotics Res..

[29]  Gamini Dissanayake,et al.  Linear SLAM: A linear solution to the feature-based and pose graph SLAM based on submap joining , 2013, 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[30]  J. A. Castellanos,et al.  Limits to the consistency of EKF-based SLAM , 2004 .

[31]  Carlos Silvestre,et al.  Globally Asymptotically Stable Sensor-Based Simultaneous Localization and Mapping , 2013, IEEE Transactions on Robotics.

[32]  Gamini Dissanayake,et al.  Observability analysis of SLAM using fisher information matrix , 2008, 2008 10th International Conference on Control, Automation, Robotics and Vision.

[33]  Philippe Martin,et al.  Invariant Extended Kalman Filter: theory and application to a velocity-aided attitude estimation problem , 2009, Proceedings of the 48h IEEE Conference on Decision and Control (CDC) held jointly with 2009 28th Chinese Control Conference.

[34]  Philippe Martin,et al.  Symmetry-Preserving Observers , 2006, IEEE Transactions on Automatic Control.

[35]  Lina María Paz,et al.  Divide and Conquer: EKF SLAM in O(n) , 2008, IEEE Trans. Robotics.

[36]  Hugh F. Durrant-Whyte,et al.  An Autonomous Guided Vehicle for Cargo Handling Applications , 1995, ISER.

[37]  Frank Dellaert,et al.  Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing , 2006, Int. J. Robotics Res..

[38]  Axel Barrau,et al.  Navigating with highly precise odometry and noisy GPS: a case study , 2016 .

[39]  Philippe Martin,et al.  Generalized Multiplicative Extended Kalman Filter for Aided Attitude and Heading Reference System , 2010 .

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