Robust Variational Bayesian Adaptive Cubature Kalman Filtering Algorithm for Simultaneous Localization and Mapping with Heavy-Tailed Noise

Simultaneous localization and mapping (SLAM) has been applied across a wide range of areas from robotics to automatic pilot. Most of the SLAM algorithms are based on the assumption that the noise is timeinvariant Gaussian distribution. In some cases, this assumption no longer holds and the performance of the traditional SLAM algorithms declines. In this paper, we present a robust SLAM algorithm based on variational Bayes method by modelling the observation noise as inverse-Wishart distribution with “harmonic mean”. Besides, cubature integration is utilized to solve the problem of nonlinear system. The proposed algorithm can effectively solve the problem of filtering divergence for traditional filtering algorithm when suffering the time-variant observation noise, especially for heavy-tailed noise. To validate the algorithm, we compare it with other traditional filtering algorithms. The results show the effectiveness of the algorithm.

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

[2]  Roland Siegwart,et al.  Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback , 2017, Int. J. Robotics Res..

[3]  Aboelmagd Noureldin,et al.  Adaptive square-root CKF based SLAM algorithm for indoor UGVs , 2017, 2017 IEEE International Conference on Mechatronics and Automation (ICMA).

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

[5]  Eduardo Mario Nebot,et al.  Approximate Inference in State-Space Models With Heavy-Tailed Noise , 2012, IEEE Transactions on Signal Processing.

[6]  Zhongliang Jing,et al.  A distributed consensus filter for sensor networks with heavy-tailed measurement noise , 2018, Science China Information Sciences.

[7]  R. A. Leibler,et al.  On Information and Sufficiency , 1951 .

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

[9]  Wolfram Burgard,et al.  Improving Data Association in Vision-based SLAM , 2006, 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[10]  Randall Smith,et al.  Estimating Uncertain Spatial Relationships in Robotics , 1987, Autonomous Robot Vehicles.

[11]  R. Weinstock.  Calculus of Variations: with Applications to Physics and Engineering , 1952 .

[12]  Ian D. Reid,et al.  Article in Press Robotics and Autonomous Systems ( ) – Robotics and Autonomous Systems a Comparison of Loop Closing Techniques in Monocular Slam , 2022 .

[13]  Michael A. West,et al.  Dynamic matrix-variate graphical models , 2007 .

[14]  Simo Särkkä,et al.  Recursive Noise Adaptive Kalman Filtering by Variational Bayesian Approximations , 2009, IEEE Transactions on Automatic Control.

[15]  Hugh F. Durrant-Whyte,et al.  A new method for the nonlinear transformation of means and covariances in filters and estimators , 2000, IEEE Trans. Autom. Control..

[16]  Roland Siegwart,et al.  Robust visual inertial odometry using a direct EKF-based approach , 2015, IROS 2015.

[17]  Henry Leung,et al.  Robust Consensus Nonlinear Information Filter for Distributed Sensor Networks With Measurement Outliers , 2019, IEEE Transactions on Cybernetics.

[18]  Ian Postlethwaite,et al.  Cubature Kalman Filter based Localization and Mapping , 2011 .

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

[20]  John J. Leonard,et al.  Past, Present, and Future of Simultaneous Localization and Mapping: Toward the Robust-Perception Age , 2016, IEEE Transactions on Robotics.

[21]  Xin Zhang,et al.  The application of square-root cubature Kalman filter in SLAM for underwater robot , 2017, 2017 Chinese Automation Congress (CAC).

[22]  A. Nakabayashi,et al.  An Extension of the Ensemble Kalman Filter for Estimating the Observation Error Covariance Matrix Based on the Variational Bayes’s Method , 2017 .

[23]  Henry Leung,et al.  Variational Bayesian Adaptive Cubature Information Filter Based on Wishart Distribution , 2017, IEEE Transactions on Automatic Control.

[24]  Davide Scaramuzza,et al.  SVO: Fast semi-direct monocular visual odometry , 2014, 2014 IEEE International Conference on Robotics and Automation (ICRA).

[25]  Simo Särkkä,et al.  Non-linear noise adaptive Kalman filtering via variational Bayes , 2013, 2013 IEEE International Workshop on Machine Learning for Signal Processing (MLSP).

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

[27]  Jerry Nedelman,et al.  Book review: “Bayesian Data Analysis,” Second Edition by A. Gelman, J.B. Carlin, H.S. Stern, and D.B. Rubin Chapman & Hall/CRC, 2004 , 2005, Comput. Stat..

[28]  S. Haykin,et al.  Cubature Kalman Filters , 2009, IEEE Transactions on Automatic Control.

[29]  Dariusz Janczak,et al.  Estimation method for measurements with heavy-tailed noise variance , 2017 .

[30]  Sebastian Thrun,et al.  FastSLAM: a factored solution to the simultaneous localization and mapping problem , 2002, AAAI/IAAI.

[31]  Alireza Mohammad Shahri,et al.  Iterated Unscented SLAM algorithm for navigation of an autonomous mobile robot , 2008, 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[32]  Zhongliang Jing,et al.  A Consensus Nonlinear Filter With Measurement Uncertainty in Distributed Sensor Networks , 2017, IEEE Signal Processing Letters.

[33]  Daniel Cremers,et al.  Direct Sparse Odometry , 2016, IEEE Transactions on Pattern Analysis and Machine Intelligence.

[34]  Zheping Yan,et al.  An Adaptive UKF Based SLAM Method for Unmanned Underwater Vehicle , 2013 .