Redesign and analysis of globally asymptotically stable bearing only SLAM

The Simultaneous Localization And Mapping (SLAM) estimation problem is a nonlinear problem, due to the nature of the range and bearing measurements. In latter years it has been demonstrated that if the nonlinearities from the attitude are handled by a separate nonlinear observer, the SLAM dynamics can be represented as a linear time varying (LTV) system, by introducing these nonlinearities and nonlinear measurements as time varying vectors and matrices. This makes the SLAM estimation problem globally solvable with a Kalman filter, however, the noise structure is no longer trivial. In this paper, a new bearings only SLAM estimation algorithm is presented, including a novel design of the noise covariance matrices. Simulations of the SLAM estimator are presented, and show the performance of the state and uncertainty estimates, as well as the stability of the proposed estimator.

[1]  Stergios I. Roumeliotis,et al.  A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation , 2007, Proceedings 2007 IEEE International Conference on Robotics and Automation.

[2]  Carlos Silvestre,et al.  Sensor-based globally exponentially stable range-only simultaneous localization and mapping , 2015, Robotics Auton. Syst..

[3]  Gamini Dissanayake,et al.  Bearing-only SLAM Using a SPRT Based Gaussian Sum Filter , 2005, Proceedings of the 2005 IEEE International Conference on Robotics and Automation.

[4]  Gamini Dissanayake,et al.  Linear MonoSLAM: A linear approach to large-scale monocular SLAM problems , 2014, 2014 IEEE International Conference on Robotics and Automation (ICRA).

[5]  Kostas E. Bekris,et al.  Evaluation of algorithms for bearing-only SLAM , 2006, Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006..

[6]  Leigh McCue,et al.  Handbook of Marine Craft Hydrodynamics and Motion Control [Bookshelf] , 2016, IEEE Control Systems.

[7]  Hugh Durrant-Whyte,et al.  Simultaneous localization and mapping (SLAM): part II , 2006 .

[8]  Carlos Silvestre,et al.  Single range aided navigation and source localization: Observability and filter design , 2011, Syst. Control. Lett..

[9]  Thor I. Fossen,et al.  Handbook of Marine Craft Hydrodynamics and Motion Control: Fossen/Handbook of Marine Craft Hydrodynamics and Motion Control , 2011 .

[10]  Tor Arne Johansen,et al.  Globally exponentially stable Kalman filtering for SLAM with AHRS , 2016, 2016 19th International Conference on Information Fusion (FUSION).

[11]  B. Anderson,et al.  Controllability, Observability and Stability of Linear Systems , 1968 .

[12]  J. S. Ortega.  Quaternion kinematics for the error-state KF , 2016 .

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

[14]  Carlos Silvestre,et al.  3-D inertial trajectory and map online estimation: Building on a GAS sensor-based SLAM filter , 2013, 2013 European Control Conference (ECC).

[15]  Tor Arne Johansen,et al.  Attitude Estimation Based on Time-Varying Reference Vectors with Biased Gyro and Vector Measurements , 2011 .

[16]  B. Anderson.  Stability properties of Kalman-Bucy filters , 1971 .

[17]  Thia Kirubarajan,et al.  Estimation with Applications to Tracking and Navigation: Theory, Algorithms and Software , 2001 .

[18]  Neil Genzlinger.  A. and Q , 2006 .

[19]  Jeffrey J. DaCunha,et al.  Transition matrix and generalized matrix exponential via the Peano-Baker series , 2005 .

[20]  R. Kálmán.  Mathematical description of linear dynamical systems , 1963 .

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

[22]  Nicholas Roy,et al.  Optimization-Based Estimator Design for Vision-Aided Inertial Navigation , 2013 .

[23]  Thomas Kailath,et al.  Linear Systems , 1980 .