An Improved FastSLAM Algorithm Based on Revised Genetic Resampling and SR-UPF

  • Tai-Zhi Lv
  • Chun-Xia Zhao
  • Hao-Feng Zhang
Research Article


FastSLAM is a popular framework which uses a Rao-Blackwellized particle filter to solve the simultaneous localization and mapping problem (SLAM). However, in this framework there are two important potential limitations, the particle depletion problem and the linear approximations of the nonlinear functions. To overcome these two drawbacks, this paper proposes a new FastSLAM algorithm based on revised genetic resampling and square root unscented particle filter (SR-UPF). Double roulette wheels as the selection operator, and fast Metropolis-Hastings (MH) as the mutation operator and traditional crossover are combined to form a new resampling method. Amending the particle degeneracy and keeping the particle diversity are both taken into considerations in this method. As SR-UPF propagates the sigma points through the true nonlinearity, it decreases the linearization errors. By directly transferring the square root of the state covariance matrix, SR-UPF has better numerical stability. Both simulation and experimental results demonstrate that the proposed algorithm can improve the diversity of particles, and perform well on estimation accuracy and consistency.


Simultaneous localization and mapping (SLAM) genetic algorithm square root unscented particle filter (SR-UPF) fast Metropolis-Hastings (MH) double roulette wheels 


Unable to display preview. Download preview PDF.

Unable to display preview. Download preview PDF.



Tai-Zhi Lv would like to thank Jiangsu Overseas Research & Training Program for University Prominent Young & Middle-aged Teachers and Presidents for financial support.


  1. [1]
    C. L. Wang, T. M. Wang, J. H. Liang, Y. C. Zhang, Y. Zhou. Bearing-only visual SLAM for small unmanned aerial vehicles in GPS-denied environments. International Journal of Automation and Computing, vol. 10, no. 5, pp. 387–396, 2013.CrossRefGoogle Scholar
  2. [2]
    S. Thrun, W. Burgard, D. Fox. Probabilistic Robotics, Cambridge, USA: The MIT Press, 2005.zbMATHGoogle Scholar
  3. [3]
    M. Kaess, A. Ranganathan, F. Dellaert, iSAM: Incremental smoothing and mapping, Robotics, vol. 24, no. 6, pp. 1365–1378, 2008.Google Scholar
  4. [4]
    S. Thrun, M. Montemerlo. The GraphSLAM Algorithm with Application to Large-Scale Mapping of Urban Structures, The International Journal of Robotics Research, vol. 25, no. 5–6, pp. 403–429, 2006.Google Scholar
  5. [5]
    H. Wang, W. Mou, G. Seet, M. H. Li, M. W. S. Lau, D. W. Wang. Real-time visual odometry estimation based on principal direction detection on ceiling vision. International Journal of Automation and Computing, vol. 10, no. 5, pp. 397–404, 2013.CrossRefGoogle Scholar
  6. [6]
    M. Montemerlo. FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem with Unknown Data Association, Ph.D. dissertation, Carnegie Mellon University, USA, 2003.Google Scholar
  7. [7]
    C. Gamallo, M. Mucientes, C. V. Regueiro. A FastSLAMbased Algorithm for Omni directional Cameras, Journal of Physical Agents, vol. 7, no. 1, pp. 12–21, 2012.Google Scholar
  8. [8]
    S. M. Chen, J. F. Yuan, F. Zhang, H. J. Fang. Multirobot FastSLAM Algorithm Based on Landmark Consistency Correction, Mathematical Problems in Engineering, vol. 2014, pp. 1–7, 2014.Google Scholar
  9. [9]
    R. Martinez-Cantin, J. A. Castellanos. Unscented SLAM for large-scale outdoor environments. In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, IEEE, Edmonton, Canada, pp. 3427–3432, 2005.Google Scholar
  10. [10]
    D. B. Liu, G. R. Liu, M. H. Yu. An improved FastSLAM framework based on particle swarm optimization and unscented particle filter. Journal of Computational Information Systems, vol. 8, no. 7, pp. 2859–2866, 2012.Google Scholar
  11. [11]
    K. Chanki, R. Sakthivel, W. K. Chung. Unscented Fast- SLAM: A robust algorithm for the simultaneous localization and mapping problem. In Proceedings of International Conference on Robotics and Automation, IEEE, Roma, Italy, pp. 2439–2445, 2007.Google Scholar
  12. [12]
    J. H. Zhu, N. N. Zheng, Z. J. Yuan, Q. Zhang. A SLAM algorithm based on central difference particle filter. Acta Automatica Sinica, vol. 36, no. 2, pp. 249–257, 2010. (in Chinese)CrossRefGoogle Scholar
  13. [13]
    Y. Song, Y. D. Song, Q. L. Li. Robust iterated sigma point FastSLAM algorithm for mobile robot simultaneous localization and mapping. Chinese Journal of Mechanical Engineering, vol. 24, no. 4, pp. 693–700, 2011.CrossRefGoogle Scholar
  14. [14]
    T. Bailey, J. Nieto, E. Nebot. Consistency of the FastSLAM algorithm. In Proceedings of International Conference on Robotics and Automation, IEEE, Orlando, USA, pp. 424–429, 2006.Google Scholar
  15. [15]
    C. Stachniss, D. Hahnel, W. Burgard. Exploration with active loop-closing for FastSLAM. In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, IEEE, Sendai, Japan, pp. 1505–1510, 2004.Google Scholar
  16. [16]
    J. S. Liu. Metropolized independent sampling with comparisons to rejection sampling and importance sampling. Statistics and Computing, vol. 6, no. 2, pp. 113–119, 1996.CrossRefGoogle Scholar
  17. [17]
    G. Grisetti, C. Stachniss, W. Burgard. Improved techniques for grid mapping with Rao-Blackwellized particle filters. IEEE Transactions on Robotics, vol. 23, no. 1, pp. 34–46, 2007.CrossRefGoogle Scholar
  18. [18]
    N. Nosan, I. K. Kim, H. C. Lee, B. H. Lee. Analysis of resampling process for the particle depletion problem in fastslam. In Proceedings of the 16th International Symposium on Robot and Human Interactive Communication, IEEE, Jeju Island, Korea, pp. 200–205, 2007.Google Scholar
  19. [19]
    X. Z. Li, S. M. Jia, W. Cui. On sample diversity in particle filter based robot SLAM. In Proceedings of International Conference on Robotics and Biomimetics, IEEE, Phuket, Thailand, pp. 1072–1077, 2011.Google Scholar
  20. [20]
    Y. M. Xia, Y. M. Yang. An improved FastSLAM algorithm based on genetic algorithms. In Proceedings of International Symposium, Communications in Computer and Information Science, Springer, Guangzhou, China, pp. 296–302, 2011.Google Scholar
  21. [21]
    H. Wang, Y. Yan, D. W. Wang. A GA based SLAM with range sensors only. In Proceedings of the 11th International Conference on Control Automation Robotics and Vision, IEEE, Singapore, pp. 1796–1802, 2010.Google Scholar
  22. [22]
    P. Li, S. M. Song, X. L. Chen, G. R. Duan. Square root unscented Kalman filter incorporating Gaussian process regression. Systems Engineering and Electronics, vol. 32, no. 6, pp. 1281–1285, 2010. (in Chinese)Google Scholar
  23. [23]
    R. D. Merw, E. A. Wan. The square-root unscented kalman filter for state and parameter-estimation. Systems Engineering and Electronics, vol. 32, no. 6, pp. 1281–1285, 2010.Google Scholar
  24. [24]
    P. Giordani, R. Kohn. Adaptive independent Metropolis- Hastings by fast estimation of mixtures of normals. Journal of Computational and Graphical Statistics, vol. 19, no. 2, pp. 243–259, 2010.MathSciNetCrossRefGoogle Scholar
  25. [25]
    J. E. Guivant, E. M. Nebot. Optimization of the simultaneous localization and map-building algorithm for real-time implementation. IEEE Transactions on Robotics and Automation, vol. 17, no. 3, pp. 242–257, 2001.CrossRefGoogle Scholar
  26. [26]
    T. Bailey. SLAM simulations, [Online], Available:, March 10, 2011.Google Scholar
  27. [27]
    Y. Bar-Shalom, X. R. Li, T. Kirubarajan. Estimation with Applications to Tracking and Navigation: Theory Algorithms and Software, Hoboken, USA: John Wiley and Sons, 2004.Google Scholar
  28. [28]
    E. Nebot, J Guivant, J. Nieto. ACFR, Experimental outdoor dataset, [Online], Available: http://www.acfr.usyd., July 28, 2012.Google Scholar

Copyright information

© Institute of Automation, Chinese Academy of Sciences and Springer-Verlag GmbH Germany, part of Springer Nature 2015

Authors and Affiliations

  1. 1.School of Computer Science and TechnologyNanjing University of Science and TechnologyNanjingChina

Personalised recommendations