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

doi:10.1007/s12204-019-2146-7

Abstract

Abstract: 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 time- invariant 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 e?ectively solve the problem of ˉltering divergence for traditional ˉltering algorithm when su?ering the time-variant obser- vation noise, especially for heavy-tailed noise. To validate the algorithm, we compare it with other traditional ˉltering algorithms. The results show the e?ectiveness of the algorithm.

Key words: simultaneous localization and mapping (SLAM)| variational Bayesian (VB)| heavy-tailed noise| robust estimation

摘要： 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 time- invariant 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 e?ectively solve the problem of ˉltering divergence for traditional ˉltering algorithm when su?ering the time-variant obser- vation noise, especially for heavy-tailed noise. To validate the algorithm, we compare it with other traditional ˉltering algorithms. The results show the e?ectiveness of the algorithm.

关键词: simultaneous localization and mapping (SLAM)| variational Bayesian (VB)| heavy-tailed noise| robust estimation

CLC Number:

TP 242

ZHANG Zhuqing (张铸青), DONG Peng (董鹏), TUO Hongya (庹红娅), LIU Guangjun (刘光军), JIA He (贾鹤). Robust Variational Bayesian Adaptive Cubature Kalman Filtering Algorithm for Simultaneous Localization and Mapping with Heavy-Tailed Noise[J]. Journal of Shanghai Jiao Tong University (Science), 2020, 25(1): 76-87.

References 37

[1]	DURRANT-WHYTE H, BAILEY T. Simultaneous localization and mapping: Part I [J]. IEEE Robotics & Automation Magazine, 2006, 13(2): 99-107.
[2]	BAILEY T, DURRANT-WHYTE H. Simultaneous localization and mapping (SLAM): Part II [J]. IEEE Robotics & Automation Magazine, 2006, 13(3): 108-117.
[3]	CADENA C, CARLONE L, CARRILLO H, et al.Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age [J].IEEE Transactions on Robotics, 2016, 32(6): 1309-1332.
[4]	MOURIKIS A I, ROUMELIOTIS S I. A multistate constraint Kalman ˉlter for vision-aided inertial navigation [C]//IEEE International Conference on Robotics and Automation. Rome, Italy: IEEE, 2007:3565-3572.
[5]	BLOESCHM, BURRI M, OMARI S, et al. Iterated extended Kalman ˉlter based visual-inertial odometry using direct photometric feedback [J]. The International Journal of Robotics Research, 2017, 36(10):1053-1072.
[6]	SHOJAIE K, SHAHRI A M. Iterated unscented SLAM algorithm for navigation of an autonomous mobile robot [C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Nice, France: IEEE,2008: 1582-1587.
[7]	WANG H J, FU G X, LI J, et al. An adaptive UKF based SLAM method for unmanned underwater vehicle [J]. Mathematical Problems in Engineering, 2013,2013: 605981.
[8]	BLOESCH M, OMARI S, HUTTER M, et al. Robust visual inertial odometry using a direct EKF-based approach [C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Hamburg, Germany:IEEE, 2015: 298-304.
[9]	XIA J Z, IQBAL U, NOURELDIN A, et al. Adaptive square-root CKF based SLAM algorithm for indoor UGVs [C]//IEEE International Conference on Mechatronics and Automation. Takamatsu, Japan: IEEE,2017: 1942-1946.
[10]	CHANDRA K P B, GU D W, POSTLETHWAITE I.Cubature Kalman ˉlter based localization and mapping [J]. IFAC Proceedings Volumes, 2011, 44(1):2121-2125.
[11]	ENGEL J, KOLTUN V, CREMERS D. Direct sparse odometry [J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2018, 40(3): 611-625.
[12]	MUR-ARTAL R, MONTIEL J M M, TARD?OS J D. ORB-SLAM: A versatile and accurate monocular SLAM system [J]. IEEE Transactions on Robotics,2015, 31(5): 1147-1163.
[13]	FORSTER C, PIZZOLI M, SCARAMUZZA D.SVO: Fast semi-direct monocular visual odometry[C]//IEEE International Conference on Robotics and Automation. Hong Kong, China: IEEE, 2014: 15-22.
[14]	SMITH R, SELF M, CHEESEMAN P. Estimating uncertain spatial relationships in robotics [C]//IEEE International Conference on Robotics and Automation.Raleigh, NC, USA: IEEE, 1987: 850-850.
[15]	WILLIAMS B, CUMMINS M, NEIRA J, et al. A comparison of loop closing techniques in monocular SLAM[J]. Robotics and Autonomous Systems, 2009, 57(12):1188-1197.
[16]	MONTERNERLO M, THRUN S, KOLLER D, et al.FastSLAM: A factored solution to the simultaneous localization and mapping problem [C]//The Eighteenth National Conference on Artiˉcial Intelligence and the Fourteenth Annual Conference on Innovative Applications of Artiˉcial Intelligence. Menlo Park, California,USA: AAAI, 2002: 593-598.
[17]	JULIER S, UHLMANN J, DURRANT-WHYTE H F. A new method for the nonlinear transformation of means and covariances in ˉlters and estimators[J]. IEEE Transactions on Automatic Control, 2000,45(3): 477-482.
[18]	ARASARATNAM I, HAYKIN S. Cubature Kalman filters [J]. IEEE Transactions on Automatic Control,2009, 54(6): 1254-1269.
[19]	GIL A, REINOSO ? O, MOZOS ?O M, et al. Improving data association in vision-based SLAM[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Beijing, China: IEEE, 2006:2076-2081.
[20]	NAKABAYASHI A, UENO G. An extension of the ensemble Kalman ˉlter for estimating the observation error covariance matrix based on the variational Bayes'smethod [J]. Monthly Weather Review, 2017, 145(1):199-213.
[21]	JANCZAK D. Estimation method for measurements with heavy-tailed noise variance [J]. Przegld Elektrotechniczny, 2017, 93(12): 24-27.
[22]	DONG P, JING Z L, LEUNG H, et al. Variational Bayesian adaptive cubature information ˉlter based on Wishart distribution [J]. IEEE Transactions on Automatic Control, 2017, 62(11): 6051-6057.
[23]	DONG P, JING Z L, LEUNG H, et al. Robust consensus nonlinear information ˉlter for distributed sensor networks with measurement outliers [J]. IEEE Transactions on Cybernetics, 2019, 49(10): 3731-3743.
[24]	DONG P, JING Z L, SHEN K, et al. A distributed consensus ˉlter for sensor networks with heavy-tailed measurement noise [J]. Science China Information Sciences, 2018, 61(11): 119201.
[25]	SHEN K, JING Z L, DONG P. A consensus nonlinear filter with measurement uncertainty in distributed sensor networks [J]. IEEE Signal Processing Letters,2017, 24(11): 1631-1635.
[26]	S?ARKK?A S, HARTIKAINEN J. Non-linear noise adaptive Kalman ˉltering via variational Bayes[C]//IEEE International Workshop on Machine Learning for Signal Processing. Southampton, UK:IEEE, 2013: 1-6.
[27]	S?ARKK?A S, NUMMENMAA A. Recursive noise adaptive Kalman ˉltering by variational Bayesian approximations [J]. IEEE Transactions on Automatic Control,2009, 54(3): 596-600.
[28]	AGAMENNONI G, NIETO J I, NEBOT E M. Approximate inference in state-space models with heavytailed noise [J]. IEEE Transactions on Signal Processing, 2012, 60(10): 5024-5037.
[29]	XU W J. Research on Bayesian filters based simultaneous localization and mapping algorithms for mobile robots [D]. Hangzhou, China: Zhejiang University,2016 (in Chinese).
[30]	ZHANG S Y, DONG P, JING Z L. Adaptive cubature Kalman ˉltering SLAM algorithm based on variational Bayes [J]. Journal of Harbin Institute of Technology,2019, 51(4): 12-18 (in Chinese).
[31]	WEINSTOCK R. Calculus of variations: With applications to physics and engineering [M]. New York,USA: Dover Publications, Inc, 1974.
[32]	GELMAN A, CARLIN J B, STERN H S, et al.Bayesian data analysis [M]. London, UK: Chapman & Hall, 1995.
[33]	KULLBACK S, LEIBLER R A. On information and sufficiency [J]. Annals of Mathematical Statistics, 1951,22(1): 79-86.
[34]	CARVALHO C, WEST M. Dynamic matrix-variate graphical models [J]. Bayesian Analysis, 2007, 2(1):69-98.
[35]	BAILEY T, NIETO J, GUIVANT J, et al. Consistency of the EKF-SLAM algorithm [C]//IEEE/RSJ International Conference on Intelligent Robots and Systems.Beijing, China: IEEE, 2006: 3562-3568.
[36]	MARTINEZ-CANTIN R, CASTELLANOS J A. Unscented SLAM for largescale outdoor environments[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Edmonton, Alberta, Canada:IEEE, 2005: 3427-3432.
[37]	LI X, FENG Y B, HUANG R H, et al. The application of square-root cubature Kalman ˉlter in SLAM for underwater robot [C]//2017 Chinese Automation Congress (CAC). Jinan, china: IEEE, 2017: 2183-2187.