FISST-SLAM: Finite Set Statistical Approach to Simultaneous Localization and Mapping

Abstract

The solution to the problem of mapping an environment and at the same time using this map to localize (the simultaneous localization and mapping, SLAM, problem) is a key prerequisite in the synthesis of truly autonomous vehicles. By far the most common formulation of the SLAM problem is founded on a vector based stochastic framework, where the sensor models and the vehicle models are represented in state-space form and the joint posterior or its statistics are obtained based on recursive Bayesian estimation. All of these SLAM solutions leading from the stochastic vector state-space approach require that we solve certain parallel problems in each recursion. These include effective solutions to the problems of data association, feature extraction, clutter filtering, and landmark or map management. In this paper, we propose an alternative framework based on finite set statistics (FISST), where the SLAM problem is reformulated so that the landmark map and the measurements are represented using random finite sets and the landmark map is jointly estimated with the vehicle state vector, whilst explicitly accounting for measurement detection uncertainty, data-association uncertainty, false alarms and map management in the SLAM filter framework. Similar to FastSLAM, the proposed formulation is based on a factorization of the full SLAM posterior into a product of the vehicle trajectory posterior and the landmark map posterior conditioned on the vehicle trajectory. The vehicle trajectory posterior is then estimated using a particle filter and the map posterior conditioned on the vehicle trajectory via a sequential Monte Carlo (SMC) implementation of the probability hypothesis density (PHD) filter. Simulation results of the proposed algorithm are presented and benchmarked against FastSLAM to demonstrate the effectiveness and improved performance of the FISST-SLAM in the presence of significant clutter.

Keywords

SLAM random finite sets probability hypothesis density filter

Get full access to this article

View all access options for this article.

References

Challa, S. , Vo, B. and Wang, X. ( 2002). Bayesian approaches to track existence-IPDA and random sets. Proceedings of the Fifth International Conference on Information Fusion, 2002, volume 2, pp. 1228-1235.

Clark, D.E. and Bell, J. ( 2007). Multi-target state estimation and track continuity for the particle PHD filter. IEEE Transactions on Aerospace and Electronic Systems, 43: 1441- 1453.

Dissanayake, G. , Williams, S.B. , Durrant-Whyte, H. and Bailey, T. ( 2002). Map management for efficient simultaneous localization and mapping (SLAM). Autonomous Robots, 12(3): 267-286.

Elfes, A. ( 1989). Using occupancy grids for mobile robot perception and navigation. Computer, 22(6): 46-57.

Erdinc, O. , Willett, P. and Bar-Shalom, Y. ( 2005). Probability hypothesis density filter for multitarget multisensor tracking. Proceedings of the 8th International Conference on Information Fusion (FUSION), volume 1, p. 8.

Guivant, J. and Nebot, E. ( 2001). Optimization of the simultaneous localization and map-building algorithm for real-time implementation. IEEE Transactions on Robotics and Automation, 17(3): 242-257.

Hoffman, J. and Mahler, R. ( 2004). Multitarget miss distance via optimal assignment. IEEE Transactions on Systems, Man and Cybernetics, Part A: Systems and Humans, 34(3): 327-336.

Ikoma, N. , Uchino, T. and Maeda, H. ( 2004). Tracking of feature points in image sequence by SMC implementation of PHD filter. Proceedings of the Society of Instrument and Control Engineers Annual Conference (SICE2004), Hokkaido , pp. 1696-1701.

Neira,J. and Tardos, J.D. (2001). Data association in stochastic mapping using the joint compatibility test . IEEE Transactions on Robotics and Automation, 17(6): 8890-8897.

10.

Kalyan, B. , Balasuriya, A. and Wijesoma, S. ( 2006). Multiple target tracking in underwater sonar images using Particle-PHD filter. OCEANS 2006-Asia Pacific, Singapore, pp. 1- 5.

11.

Lee, K.W. , Wijesoma, W. and Javier, I. ( 2006). On the observability and observability analysis of SLAM . IEEE/RSJ Intenational Conference on Intelligent Robots and Systems (IROS), Beijing, pp. 3569-3574.

12.

Li, X. ( 1998). Tracking in clutter with strongest neighbor measurements. i. theoretical analysis. IEEE Transactions on Automatic Control , 43(11): 1560-1578.

13.

Lin, L. , Bar-Shalom, Y. and Kirubarajan, T. ( 2006). Track labeling and PHD filter for multitarget tracking . IEEE Transactions on Aerospace and Electronic Systems, 42(3): 778- 795.

14.

Mahler, R. ( 2007a). PHD filters of higher order in target number. IEEE Transactions on Aerospace and Electronic Systems, 43(4): 1523-1543.

15.

Mahler, R.P.S. ( 2003). Multi-target Bayes filtering via first-order multi-target moments. IEEE Transactions on Aerospace and Electronic Systems , 39(4): 1152-1178.

16.

Mahler, R.P.S. ( 2007b). Statistical Multisource-Multitarget Information Fusion . Norwood, MA: Artech House Publishers .

17.

Montemerlo, M. ( 2003). FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping problem with Unknown Data Association. PhD thesis, Carnegie Mellon University, School of Computer Science.

18.

Moutarlier, P. and Chatila, R. ( 1990). An experimental system for incremental environment modelling by an autonomous mobile robot. The First International Symposium on Experimental Robotics I, pp. 327-346.

19.

Mullane, J. , Vo, B. , Adams, M.D. and Wijesoma, W.S. ( 2008). A random set formulation for bayesian SLAM. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2008), pp. 1043-1049.

20.

Murphy, K.P. ( 1999). Bayesian map learning in dynamic environments. 13th Annual Conference on Neural Information Processing Systems (NIPS), Solla, S. A., Leen, T. K. and K.-R. Müller (eds). Cambridge, MA, MIT Press, pp. 1015- 1021.

21.

Dissanayake, M.W.M.G. , Newman, P. , Clark, S. , Durrant-Whyte, H.F. and Csorba, M. ( 2001). A solution to the simultaneous localization and map building (SLAM) problem. IEEE Transactions on Robotics and Automation , 17(3): 229-241.

22.

Sidenbladh, H. and Wirkander, S. ( 2003). Tracking random sets of vehicles in terrain. Proceedings of the 2003 IEEE Workshop on Multi-Object Tracking , Madison, WI.

23.

Smith, R.C. and Cheeseman, P. ( 1987). On the representation and estimation of spatial uncertainly . The International Journal of Robotics Research, 5(4): 56-68.

24.

Se, S. , Lowe, D.G. and Little, J. ( 2002). Mobile robot localization and mapping with uncertainty using scale-invariant visual landmarks. The International Journal of Robotics Research, 21(8): 735-758.

25.

Tobias, M. and Lanterman, A.D. ( 2005). Probability hypothesis density-based multitarget tracking with bistatic range and doppler observations. IEE Proceedings Radar, Sonar and Navigation, 152(3).

26.

Zajic, T. and Mahler, R. ( 2003). A particle-systems implementation of the PHD multi-target tracking filter. Signal Processing, Sensor Fusion, and Target Recognition XII. Proceedings of SPIE, 5096: 291-299.

27.

Vo, B. , Singh, S. and Doucet, A. ( 2005). Sequential monte carlo methods for multi-target filtering with random finite sets. IEEE Transactions on Aerospace and Electronic Systems, 41(4): 1224-1245.

28.

Vo, B. , Vo, B. and Cantoni, A. ( 2007). Analytic implementations of the cardinalized probability hypothesis density filter. IEEE Transactions on Signal Processing , 55(7): 3553-3567.

29.

Wang, Y. , Wu, J. , Kassim, A.A. and Huang, W. ( 2006). Tracking a variable number of human groups in video using probability hypothesis density. 18th International Conference on Pattern Recognition, Hong Kong.

30.

Wijesoma, W.S. , Perera, L.D.L. and Adams, M.D. ( 2006). Toward multidimensional assignment data association in robot localization and mapping. IEEE Transactions on Robotics , 22(2): 350-365.