Abstract—In this paper, we propose a novel FastSLAM algorithm (named as IUFastSLAM) which is based on Rao-Blackwellized Particle Filter (RBPF) framework and uses Iterated Unscented Kalman Filter (IUKF) to estimate the landmark locations. Iterated Unscented Kalman Filter (IUKF) can improve estimation accuracy over the Extend Kalman Filter and Unscented Kalman Filter. The experimental results show that the proposed algorithm has a superior performance in estimation accuracy and prolonged consistency when compared with the FastSLAM2.0 and UFastSLAM algorithms. I. INTRODUCTION imultaneous Localization and Mapping (SLAM) is a fundamental research which empowers the robots with the ability to learn consistent map while simultaneous localizing itself in the map when it moves through an unknown environment without any prior knowledge. In the last decade, a lot of researchers have worked on the SLAM problem and gotten a notable success [1]. The most popular approaches to the SLAM problem are based on probabilistic forms and the first probabilistic framework of SLAM was introduced in 1986 [2]. The two most popular solutions to the SLAM problem are the Extend Kalman Filter SLAM (EKF-SLAM) and the Rao-Blackwellized Particle Filter (RBPF) SLAM (a.k.a., FastSLAM) [1]. EKF-SLAM estimates a fully correlated posterior over robot pose and landmarks in a map [3]. Therefore, the EKF-SLAM requires 頚岫軽 態 岻 time, where N is the number of the landmarks. The EKF-SLAM updates all landmarks and the joint covariance matrix when an observation was made. This characteristic determines that EKF-SLAM is unsuitable in real-time applications which have thousands of landmarks. Another drawback of EKF-SLAM is that EKF linearizes the motion and observation models up to first order term of Taylor series expansion. As the inherent nonlinearity in SLAM models, EKF-SLAM can’t guarantee the convergence and consistency due to errors introduced during linearization step [1], [ 4]. FastSLAM uses the Rao-Blackwellized Particle Filter to estimate the path and the map. It is based on the factorization principle that if the robot path was known, the estimation of landmarks becomes independent of each other [5], [ 6]. FastSLAM estimates the path posterior using particle Final manuscript received Oct 17, 2011. Xuejun Yan and Chunxia Zhao are with the School of Computer Science, Nanjing University of Science and Technology, Xiaolinwei Street No. 200, Nanjing 21094, China,[aimar_yxj , zhaochunxia]@126.com Jizhong Xiao is with the Dept. of Electical Engineering, The City College, City University of New York Convent Ave & 140th Street, New York, NY 10031, USA, jxiao@ccny.cuny.edu filter, estimate map features by EKFs. Particles in FastSLAM are composed of a path estimate and a set of EKF estimators of individual feature locations. As FastSLAM doesn’t need to maintain the correlated covariance matrix between landmarks, FastSLAM requires 頚岫警軽岻 time, where N is the number of landmarks and M is the number of particles. Due to the computational efficiency and successes in real-world implementations, FastSLAM draws more and more researchers’ attention. The research of FastSLAM mainly focuses on how to reduce the number of particles while maintaining the estimation accuracy and how to maintain the filter consistency of the FastSLAM in long-term [7]. A general FastSLAM framework was introduced by Montemerlo et al. [6], [7]. In this framework, each particle estimates the pose and updates the landmarks independently. The difference between the FastSLAM1.0 and 2.0 algorithms is that FastSLAM2.0 takes the observation into the proposal distribution and FastSLAM1.0 doesn’t. FastSLAM uses EKF to estimate the map and linearizes the nonlinear models to first order term of Taylor series expansion. Linearization of the motion and observation models causes inaccurate approximation of the posterior and affects the algorithm’s consistency. Unscented Kalman filter (UKF) is a natural alternative to the EKF to estimate the nonlinear systems, which was first introduced by Julier and Uhlmann [9]. UKF uses a set of deterministic sampling points (sigma points) to capture the posterior mean and covariance. This approach avoids computing the Jacobian matrices and can precisely approximate the posterior distribution up to the third order, while the EKF can only get a first order approximation [10]. UKF is already introduced into SLAM problems [11], [12] and these solutions outperform the EKF-based SLAM solutions. Although UKF has a better performance than EKF, it still suffers from statistical linear error propagation [13]. Therefore, an extension of UKF called Iterated Unsented Kalman Filter (IUKF) was introduced in the literature [14] and already applied to SLAM problems [15]. This paper proposes an improved Unsented FastSLAM (UFastSLAM) algorithm which was introduced by Chanki Kim et al. [12]. We substitute Iterated Unscented Kalman Filter (IUKF) for Unscented Kalman Filter (UKF) and call it Iterated Unscented FastSLAM (IUFastSLAM) algorithm. The experimental results show that the IUFastSLAM algorithm yields more accurate estimation and can prolong consistency for longer time than previous FastSLAM2.0 and UFastSLAM. The rest of the paper is organized as follows. Section C describes the UFastSLAM framework briefly. In section D, A noxel FastSLAM Algorithm based on Iterated Unscented Kalman Filter Xuejun Yan, Chunxia Zhao, and Jizhong Xiao, Senior Member, IEEE S ;9:/3/6799/435:/21331&48022 Æ 4233 KGGG 1906 Proceedings of the 2011 IEEE International Conference on Robotics and Biomimetics December 7-11, 2011, Phuket, Thailand