Skip to main content
SpringerLink
Log in

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

  • Published:
Journal of Shanghai Jiaotong University (Science) Aims and scope Submit manuscript

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 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.

This is a preview of subscription content, log in via an institution to check access.

Access this article

Log in via an institution

Price excludes VAT (USA)
Tax calculation will be finalised during checkout.

Instant access to the full article PDF.

Institutional subscriptions

Similar content being viewed by others

References

  1. DURRANT-WHYTE H, BAILEY T. Simultaneous localization and mapping: Part I [J]. IEEE Robotics & Automation Magazine, 2006, 13(2): 99–107.

    Article  Google Scholar 

  2. BAILEY T, DURRANT-WHYTE H. Simultaneous localization and mapping (SLAM): Part II [J]. IEEE Robotics & Automation Magazine, 2006, 13(3): 108–117.

    Article  Google Scholar 

  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.

    Article  Google Scholar 

  4. MOURIKIS A I, ROUMELIOTIS S I. A multi-state constraint Kalman filter for vision-aided inertial navigation [C]// IEEE International Conference on Robotics and Automation. Rome, Italy: IEEE, 2007: 3565–3572.

    Google Scholar 

  5. BLOESCHM, BURRI M, OMARI S, et al. Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback [J]. The International Journal of Robotics Research, 2017, 36(10): 1053–1072.

    Article  Google Scholar 

  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.

    Google Scholar 

  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.

    MathSciNet  MATH  Google Scholar 

  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.

    Google Scholar 

  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.

    Google Scholar 

  10. CHANDRA K P B, GU D W, POSTLETHWAITE I. Cubature Kalman filter based localization and mapping [J]. IFAC Proceedings Volumes, 2011, 44(1): 2121–2125.

    Article  Google Scholar 

  11. ENGEL J, KOLTUN V, CREMERS D. Direct sparse odometry [J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2018, 40(3): 611–625.

    Article  Google Scholar 

  12. MUR-ARTAL R, MONTIEL J M M, TARDÓS J D. ORB-SLAM: A versatile and accurate monocular SLAM system [J]. IEEE Transactions on Robotics, 2015, 31(5): 1147–1163.

    Article  Google Scholar 

  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.

    Google Scholar 

  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.

    Google Scholar 

  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.

    Article  Google Scholar 

  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 Artificial Intelligence and the Fourteenth Annual Conference on Innovative Applications of Artificial Intelligence. Menlo Park, California, USA: AAAI, 2002: 593–598.

    Google Scholar 

  17. JULIER S, UHLMANN J, DURRANT-WHYTE H F. A new method for the nonlinear transformation of means and covariances in filters and estimators [J]. IEEE Transactions on Automatic Control, 2000, 45(3): 477–482.

    Article  MathSciNet  Google Scholar 

  18. ARASARATNAM I, HAYKIN S. Cubature Kalman filters [J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254–1269.

    Article  MathSciNet  Google Scholar 

  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.

    Google Scholar 

  20. NAKABAYASHI A, UENO G. An extension of the ensemble Kalman filter for estimating the observation error covariance matrix based on the variational Bayes’s method [J]. Monthly Weather Review, 2017, 145(1): 199–213.

    Article  Google Scholar 

  21. JANCZAK D. Estimation method for measurements with heavy-tailed noise variance [J]. Przegld Elektrotechniczny, 2017, 93(12): 24–27.

    Article  Google Scholar 

  22. DONG P, JING Z L, LEUNG H, et al. Variational Bayesian adaptive cubature information filter based on Wishart distribution [J]. IEEE Transactions on Automatic Control, 2017, 62(11): 6051–6057.

    Article  MathSciNet  Google Scholar 

  23. DONG P, JING Z L, LEUNG H, et al. Robust consensus nonlinear information filter for distributed sensor networks with measurement outliers [J]. IEEE Transactions on Cybernetics, 2019, 49(10): 3731–3743.

    Article  Google Scholar 

  24. DONG P, JING Z L, SHEN K, et al. A distributed consensus filter for sensor networks with heavy-tailed measurement noise [J]. Science China Information Sciences, 2018, 61(11): 119201.

    Article  MathSciNet  Google Scholar 

  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.

    Article  Google Scholar 

  26. SÄRKKÄ S, HARTIKAINEN J. Non-linear noise adaptive Kalman filtering via variational Bayes [C]// IEEE International Workshop on Machine Learning for Signal Processing. Southampton, UK: IEEE, 2013: 1–6.

    Google Scholar 

  27. SÄRKKÄ S, NUMMENMAA A. Recursive noise adaptive Kalman filtering by variational Bayesian approximations [J]. IEEE Transactions on Automatic Control, 2009, 54(3): 596–600.

    Article  MathSciNet  Google Scholar 

  28. AGAMENNONI G, NIETO J I, NEBOT E M. Approximate inference in state-space models with heavy-tailed noise [J]. IEEE Transactions on Signal Processing, 2012, 60(10): 5024–5037.

    Article  MathSciNet  Google Scholar 

  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).

    Google Scholar 

  30. ZHANG S Y, DONG P, JING Z L. Adaptive cubature Kalman filtering SLAM algorithm based on variational Bayes [J]. Journal of Harbin Institute of Technology, 2019, 51(4): 12–18 (in Chinese).

    Google Scholar 

  31. WEINSTOCK R. Calculus of variations: With applications to physics and engineering [M]. New York, USA: Dover Publications, Inc, 1974.

    MATH  Google Scholar 

  32. GELMAN A, CARLIN J B, STERN H S, et al. Bayesian data analysis [M]. London, UK: Chapman & Hall, 1995.

    Book  Google Scholar 

  33. KULLBACK S, LEIBLER R A. On information and sufficiency [J]. Annals of Mathematical Statistics, 1951, 22(1): 79–86.

    Article  MathSciNet  Google Scholar 

  34. CARVALHO C, WEST M. Dynamic matrix-variate graphical models [J]. Bayesian Analysis, 2007, 2(1): 69–98.

    Article  MathSciNet  Google Scholar 

  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.

    Google Scholar 

  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.

    Google Scholar 

  37. LI X, FENG Y B, HUANG R H, et al. The application of square-root cubature Kalman filter in SLAM for underwater robot [C]// 2017 Chinese Automation Congress (CAC). Jinan, china: IEEE, 2017: 2183–2187.

    Chapter  Google Scholar 

Download references

Author information

Authors and Affiliations

  1. School of Aeronautics and Astronautics, Shanghai Jiao Tong University, Shanghai, 200240, China

    Zhuqing Zhang  (张铸青), Peng Dong  (董 鹏), Hongya Tuo  (庹红娅) & He Jia  (贾 鹤)

  2. Department of Aerospace Engineering, Ryerson University, Toronto, Canada

    Guangjun Liu  (刘光军)

Authors
  1. Zhuqing Zhang  (张铸青)
    View author publications

    You can also search for this author in PubMed Google Scholar

  2. Peng Dong  (董 鹏)
    View author publications

    You can also search for this author in PubMed Google Scholar

  3. Hongya Tuo  (庹红娅)
    View author publications

    You can also search for this author in PubMed Google Scholar

  4. Guangjun Liu  (刘光军)
    View author publications

    You can also search for this author in PubMed Google Scholar

  5. He Jia  (贾 鹤)
    View author publications

    You can also search for this author in PubMed Google Scholar

Corresponding author

Correspondence to Peng Dong  (董 鹏).

Additional information

Foundation item: The National Natural Science Foundation of China (No. 61803260)

Rights and permissions

Reprints and permissions

About this article

Check for updates. Verify currency and authenticity via CrossMark

Cite this article

Zhang, Z., Dong, P., Tuo, H. et al. Robust Variational Bayesian Adaptive Cubature Kalman Filtering Algorithm for Simultaneous Localization and Mapping with Heavy-Tailed Noise. J. Shanghai Jiaotong Univ. (Sci.) 25, 76–87 (2020). https://doi.org/10.1007/s12204-019-2146-7

Download citation

  • Received:

  • Published:

  • Issue Date:

  • DOI: https://doi.org/10.1007/s12204-019-2146-7

Key words

CLC number

Document code

Search

Navigation