11IUFast SLAM

Proceedings of the 2011 IEEE International Conference on Robotics and Biomimetics December 7-11, 2011, Phuket, Thailand A noxel FastSLAM Algorithm based on Iterated Unscented Kalman Filter Xuejun Yan, Chunxia Zhao, and Jizhong Xiao, Senior Member, IEEE " 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 S 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, [email protected] ;9:/3/6799/435:/21331&48022"Æ"4233"KGGG 1906 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, the Iterated Unscented FastSLAM is presented. In section E, experimental results are presented to show IUFastSLAM algrothm’s performance. SectionF concludes the paper. II. THE UFASTSLAM FRAMEWORK From a probabilistic point of view, SLAM problems can be regarded as a problem of calculating the posterior of the whole path and the map (1) 喧岫捲怠 痛" 兼" 権怠 痛 " 憲怠 痛 " 潔怠 痛 岻" where the path of mobile robots is denoted by ®怠 担" and the map is denoted by Œ 噺 " Œ怠 橋 Œ樽 . 怠 担 is a sequence of observations and ­怠 担 is a sequence of control inputs. ˚怠 担 噺 ˚怠 橋 ˚担 is data association variable, which indicates the correspondence between landmarks and observations from time 1 to time t. As the conditional independence between features if the robot pose is given, FastSLAM factors the posterior 喧岫捲怠 痛" 兼" 権怠 痛 " 憲怠 痛 " 潔怠 痛 岻 " 噺 "喧岫捲怠 痛" " "権怠 痛 " 憲怠 痛 " 潔怠 痛 岻 (2) 筏 " テ朝 津退怠 喧岫兼津 " 捲怠 痛" 権怠 痛 " 憲怠 痛 " 潔怠 痛 岻" So the FastSLAM posterior is decomposed into one path estimator and N feature estimators conditioned on the path [16]. FastSLAM uses Rao-Blackwellized Particle Filter to estimate the robot pose recursively, and then analytically calculates the feature locations individually by EKFs. Each particle is composed of a robot path and a set of feature locations with their covariance 岷陳峅 岷陳峅 岷陳峅 岷陳峅 岷陳峅 岷陳峅 (3) 隙痛 噺 " 峽捲怠 痛 "航怠 "デ怠 橋"" 航朝 "デ朝 峺"" 岷鱈峅 Here [m] is the index of the particles, ®怠 担 is its path and 岷鱈峅 and ""デ樽 are the mean and covariance of the nth landmark location, attached to the mth particle. It should be noted that the FastSLAM1.0 algorithm’s proposal distribution considers only the control input 憲怠 痛 and new pose sampled from the motion model 岷陳峅 岷陳峅 捲痛 ｂ喧岾捲痛 "嵳捲痛貸怠 " 憲痛 峇. Whereas FastSLAM2.0 adds the current observation into the proposal distribution, and its sampling distribution is of the form 岷陳峅 岷陳峅 (4) 捲痛 ｂ喧岾捲痛 "嵳捲痛貸怠 " "憲痛 " 権痛 " 潔痛 "峇" As a result, FastSLAM2.0 is more efficient than FastSLAM1.0 in all aspects. Based on FastSLAM2.0, Chanki Kim et al. substitute scaled unscented transformation (SUT) for Extended Kalman Filter to estimate the SLAM state, and propose the Unscented FastSLAM (UFastSLAM) algorithm [12]. Like FastSLAM, UFastSLAM consists of three parts: the mobile robot path estimation, the landmark location estimation and the importance weights calculation. 岷鱈峅 "ヅ樽 A. Path Estimation UFastSLAM samples the mobile robot pose from the proposal distribution in (4) which is regarded as optimal proposal distribution. This proposal distribution can approximate the true posterior well as the current observation was introduced into it. Instead of using EKF to estimate the robot path in each particle, UFastSLAM uses UKF to estimate the path, which avoids the Jacobian calculation in 1907 EKF and achieves accurate linearization of the nonlinear motion and observation models up to third order term of Taylor series expansion. Since the observation is not obtained in each time step of SLAM, the original UKF [9] should be modified for SLAM solutions. Several prediction steps and one update step compose one modified UKF loop in UFastSLAM. In the prediction step, the robot pose vector is augmented with the control input 憲痛 岷陳峅 岷陳峅 岷陳峅 岷陳峅 鶏 ど 捲凋通直 痛貸怠 噺 峪捲痛貸怠 崋"" 鶏凋通直 痛貸怠 噺 峪 痛貸怠 崋" (5) 憲痛 ど 芸痛 岷陳峅 岷陳峅 where 捲凋通直 痛貸怠 is the augumented pose vector and 捲痛貸怠 is the robot pose at time t-1. 芸痛 is the covariance of control input 憲痛 . A symmetric set of sigma points is sampled from the 岷陳峅 augmented vector 捲凋通直 痛貸怠 and the sigma points are given by 岷陳峅 岷陳峅 鋼待 凋通直 痛貸怠 噺 捲凋通直 痛貸怠 岷陳峅 岷陳峅 岷陳峅 鋼沈 凋通直 痛貸怠 噺 捲凋通直 痛貸怠 髪 峭謬岫詣 髪 膏岻鶏凋通直 痛貸怠 嶌 " 件 噺 な 橋 詣 岷陳峅 鋼沈 凋通直 痛貸怠 噺 岷陳峅 捲凋通直 痛貸怠 伐 峭謬岫詣 髪 岷陳峅 膏岻鶏凋通直 痛貸怠 嶌 沈 沈 " "" 件 噺 詣 髪 な 橋 に詣 膏 膏 拳待 陳 噺 """" """"拳待 頂 噺 髪 岫な 伐 糠 態 髪 紅岻 詣髪膏 詣髪膏 な 拳沈 陳 噺 " 拳沈 頂 噺 """"""""""件 噺 な 橋 に詣 に岫詣 髪 膏岻 岷陳峅 Here, the L is the dimension of the 捲凋通直 痛貸怠 and 岷陳峅 峭謬岫詣 髪 膏岻鶏凋通直 痛貸怠 嶌 is the ith row or column of the matrix 沈 岷陳峅 square root of 岫詣 髪 膏岻鶏凋通直 痛貸怠 . The 膏 is calculated by 噺 糠 態 岫詣 髪 腔岻 伐 詣 and 糠岫な 伴 糠 伴 ど岻 is an auxiliary variable to scale the sigma point set. The variable 腔"岫腔 半 ど岻 helps to guarantee the positive semidefiniteness of the covariance matrix, and usually set to 0. The 紅 contains higher order information of the posterior, and set to 0 for Gaussian distribution. Each sigma point contains the robot pose and a control input, which can be represented in the form 岷陳峅 岷陳峅 捲 (6) 鋼沈 凋通直 痛貸怠 噺 " 峪 沈 痛貸怠 崋"" 憲沈 痛 The pose state is transformed by the nonlinear motion function 訣岫 岻 of each sigma point and the prediction pose and its covariance is given by 岷陳峅 岷陳峅 (7) 鋼違沈 痛 噺 訣岾捲沈 痛貸怠 憲沈 痛 峇" 岷陳峅 岷陳峅 捲痛 痛貸怠 噺 " デ態挑 沈退待 拳沈 陳 鋼違沈 痛 岷陳峅 鶏痛 痛貸怠 岷陳峅 デ態挑 沈退待 拳沈 陳 岾鋼違沈 痛 (8) 岷陳峅 岷陳峅 捲痛 痛貸怠 峇岾鋼違沈 痛 岷陳峅 捲痛 痛貸怠 峇 脹 噺 伐 伐 " (9) When landmarks were observed, the observation is 岷陳峅 employed to update the predicted pose mean 捲痛 痛貸怠 and the 岷陳峅 covariance 鶏痛 痛貸怠 . The update step is as follows: 岷陳峅 岷陳峅 拍沈岷陳峅 軽 痛 噺 月岾鋼違沈 痛 " "航頂 痛 峇"" 岷陳峅 券博痛 岷陳峅 鶏痛 津津 噺 噺 拍 岷陳峅 デ態挑 沈退待 拳沈 陳 軽沈 痛 "" 拍 岷陳峅 デ態挑 沈退待 拳沈 頂 岾軽沈 痛 伐 (10) (11) 岷陳峅 拍沈岷陳峅 券博痛 峇岾軽 痛 伐 岷陳峅 券博痛 峇 脹 " 髪 迎痛 (12) 脹 岷陳峅 岷陳峅 岷陳峅 拍 岷陳峅 博 痛岷陳峅 峇 " 鶏痛 掴津 噺 デ態挑 沈退待 拳沈 頂 岾鋼違沈 痛 伐 捲痛 痛貸怠 峇岾軽沈 痛 伐 券 岷陳峅 計痛 岷陳峅 岷陳峅 噺 鶏痛 掴津 岾鶏痛 津津 峇 岷陳峅 噺 " 捲痛 痛貸怠 貸怠 (13) (14) 岷陳峅 伐 券博痛 峇" 髪 脹 岷陳峅 岷陳峅 岷陳峅 岷陳峅 岷陳峅 鶏痛 噺 鶏痛 痛貸怠 伐 計痛 鶏痛 津津 岾計痛 峇 " 岷陳峅 It should be noted that "航頂 痛 is 岷陳峅 捲痛 岷陳峅 計痛 岾権痛 (15) (16) the mean of the 潔建月 landmark existed in the robot map, and 月岫 岻 is the nonlinear observation model function. The 迎痛 in (12) the observation noise covariance. When no landmark is observed, we estimate the robot path without the update step. Furthermore, if several landmarks are observed simultaneously, the update step is repeated for each observed landmark and the new sigma point set in (10) is needed to produce from the updated mean and covariance in (15) and (16). At last, the pose of each particle is sampled from the Gaussian distribution: 岷陳峅 岷陳峅 岷陳峅 (17) 捲痛 ｂ軽岾捲痛 鶏痛 峇" B. Landmark Estimation The landmark update step uses the observation to update the location of the landmarks that already registered in the map. A set of sigma points are sampled from the observed landmark: 岷陳峅 岷陳峅 鋼待 噺 "航頂 痛貸怠 岷陳峅 "鋼沈 岷陳峅 鋼沈 岷陳峅 岷陳峅 噺 "航頂 痛貸怠 髪 峭謬岫詣 髪 膏岻鶏頂 痛貸怠 嶌 ""岫件 噺 な 橋 詣岻 噺 岷陳峅 "航頂 痛貸怠 岷陳峅 伐 峭謬岫詣 髪 岷陳峅 膏岻鶏頂 痛貸怠 嶌 沈 沈 ""岫件 噺 詣 髪 な 橋 に詣岻 Where "航頂 痛貸怠 is the mean of the 潔建月 landmark in map and 岷陳峅 岷陳峅 鶏頂 痛貸怠 is its covariance. The L is the dimension of"航頂 痛貸怠 . Therefore, the landmark update is as follows: 岷鱈峅 岷陳峅 岷陳峅 <博辿 担 噺 ̶岾捲痛 鋼沈 峇 岷陳峅 権痛 沈退待 脹 岷陳峅 岷陳峅 岷陳峅 岷陳峅 "航頂 痛 岷陳峅 権痛 岷陳峅 鶏頂 痛 岷陳峅 噺 岷陳峅 噺 " "航頂 痛貸怠 岷陳峅 鶏頂 痛貸怠 伐 脹 岷鱈峅 岷陳峅 岷陳峅 伐 "航頂 痛貸怠 峇岾<博辿 担 伐 権痛 峇 """ 岷陳峅 計佃 痛 噺 鶏痛 禎佃 岾鶏痛 佃佃 峇 髪 貸怠 岷陳峅 計痛 岾権痛 (19) 伐 岷陳峅 権痛 峇" 脹 岷陳峅 岷陳峅 岷陳峅 計佃 痛 鶏痛 佃佃 岾計佃 痛 峇 岷陳峅 噺 嵳に講詣痛 嵳 where 貸岫怠エ態岻 岷陳峅 脹 岷陳峅 貸怠 筏 結捲喧 犯伐岫なエに岻岾権痛 伐 権痛 峇 詣痛 岷陳峅 岷陳峅 脹 岷陳峅 貸怠 岷陳峅 岷陳峅 岾権痛 伐 権痛 峇般 (22) 岷陳峅 (23) 詣痛 噺 岾鶏痛 掴津 峇 岾鶏痛 峇 鶏痛 掴津 " 髪 鶏痛 佃佃 " This importance weight equation is similar to that in FastSALM2.0, the mathematical derivation of the relationship between them will show in section D. This section briefly describes the UFastSLAM as the basis for our new algorithm. The feature initialization step and the full description of UFastSLAM are shown in [12]. III. ITERATED UNSCENTED FASTSLAM FRAMEWORK In this section we introduce the Iterated Unscented Kalman Filter (IUKF) and use IUKF to improve the UFastSLAM framework and propose the IUFastSLAM framework. A. Iterated Unscented Estimation in IUFastSLAM IUKF is an extension of UKF, and the original non-iterated UKF method can also be regarded as an IUKF with one iteration. The IUKF can be derived from the Iterated Extended Kalman Filter (IEKF) [14], as it is a statistical linearization problem with linear error propagation. We rewrite the update step of the path estimation section into the IEKF forms: 岷陳峅 岷陳峅 岷陳峅 (24) 券博痛 噺 "月岾捲沈 賃 " "航頂 痛 峇" 岷陳峅 捲沈袋怠 賃 岷陳峅 岷陳峅 計 噺 鶏痛 痛貸怠 茎沈脹掴 岾茎沈" 掴 鶏痛 痛貸怠 茎沈脹掴 髪 迎痛 峇 岷陳峅 岷陳峅 岷陳峅 噺 捲痛 痛貸怠 髪 計 磐権痛 伐 券博痛 伐 茎沈脹掴 岾捲痛 痛貸怠 Here, 茎沈" 掴 is the Jacobian matrix of 月岫 岻 貸怠 伐 (25) 岷陳峅 捲沈 賃 峇卑" (26) with respect to 捲 and the particle index m is omitted here. When finish one 岷陳峅 岷陳峅 iteration, 捲沈袋怠 賃 is set to 捲沈 賃 for the next iteration where i is 岷陳峅 岷陳峅 岷陳峅 岷陳峅 岷陳峅 博 岷鱈峅 博 岷鱈峅 鶏痛 佃佃 噺 デ態挑 沈退待 拳沈 頂 岾<辿 担 伐 権痛 峇岾<辿 担 伐 権痛 峇 """ 髪 迎痛 " (18) 鶏痛 禎佃 噺 デ態挑 沈退待 拳沈 頂 岾鋼沈 岷陳峅 拳痛 the index of iterate sequence. On the first iteration, 捲怠 賃 is set 態挑 岷陳峅 噺 布 拳沈 陳 傑沈違 痛 form: (20) (21) is the predicted observation of the associated Here, landmark. When a landmark is observed for the first time, an initialization step is required. As the initialization step in our algorithm is the same as it in UFastSLAM [12], we do not describe this step here. C. Importance Weight Calculation The importance weight of UFastSLAM is calculated by the 1908 to 捲痛 痛貸怠 and IEKF degenerates to EKF. After convergence, the updated covariance is given by 岷陳峅 岷陳峅 岷陳峅 鶏痛 噺 " 鶏痛 痛貸怠 伐 計茎沈" 頂 鶏痛 痛貸怠 "" (27) The number of iterations is usually set to 3ｂ5. We set it as 3 for high speed process in our experiments in section E. As the IEKF linearizes the non-linear models, the linear 岷陳峅 error propagation is existed in (25) as 鶏痛 痛貸怠 茎沈脹頂 . The observation model in (24) can be linearized to a linear function: 岷陳峅 岷陳峅 券博痛 噺 茎沈" 掴 捲沈 賃 髪 畦 Where, A is a constant matrix. So the propagated covariance of observation n can be calculated: 岷陳峅 潔剣懸岫券岻 噺 "継 釆岾券博痛 岷陳峅 岷陳峅 伐 権峇岾券博痛 脹 伐 権峇 挽 岷陳峅 脹 噺 継 釆岾茎沈" 掴 捲沈 賃 伐 茎沈" 掴 捲痛追通勅 峇岾茎沈" 掴 捲沈 賃 伐 茎沈" 掴 捲痛追通勅 峇 挽 噺 茎沈" 掴 潔剣懸岫捲岻茎沈" 掴 岷陳峅 噺 茎沈" 掴 鶏痛 痛貸怠 茎沈" 掴 and the cross-covariance can be calculated in the same way 岷陳峅 潔剣懸岫捲 券岻 噺 鶏痛 痛貸怠 茎沈" 掴 So (25) and (26) can be rewritten as follows: 計 噺 潔剣懸岫捲 券岻岫潔剣懸岫券岻 髪 迎痛 岻貸怠 " 岷陳峅 岷陳峅 捲沈袋怠 賃 噺 捲痛 痛貸怠 髪 計 筏 峭権痛 伐 岷陳峅 券博痛 伐 潔剣懸岫捲 岷陳峅 券岻 岾鶏痛 痛貸怠 峇 貸怠 岷陳峅 岾捲痛 痛貸怠 伐 IV. EXPERIMENTAL RESULTS (28) 岷陳峅 捲沈 賃 峇嶌 (29) As the 潔剣懸岫捲 券岻 and 潔剣懸岫券岻 can be estimated in SUT like we do in (12) and (13), an IUKF update step of the path estimation can be described by (10), (11), (12), (13), (28), (29) and (16). The IUFastSLAM is based on the UFastSLAM framework, and improved the update step of path estimation and landmark update step in UFastSLAM by an iterated form. The iterated update step of landmark estimation rewrites the (20) to the following equation 岷陳峅 岷陳峅 岷陳峅 "航沈袋怠 頂 痛 噺 " "航頂 痛貸怠 髪 計痛 岷陳峅 筏 峭権痛 伐 権痛 岷陳峅 脹 貸怠 岷陳峅 岷陳峅 In order to evaluate the performance of the IUFastSLAM algorithm, experimental results are shown in this section. A. Evaluation of Pose Estimation Accuracy T. Bailey et al. developed an opened SLAM simulator [18]. This simulator makes it possible to evaluate the performance of different SLAM algorithms. We compare the IUFastSLAM with FastSLAM2.0 and UFastSLAM using this simulator. The experiment environment is shown in Fig. 1. The robot has a 4m wheel base and uses velocity motion model 岷陳峅 伐 鶏痛 禎佃 岾鶏頂 痛貸怠 峇 岾"航頂 痛貸怠 伐 "航沈 頂 痛 峇嶌"(30) B. The Importance Weight Derivation In FastSLAM2.0 [7], the importance weight calculation is given by: 岷陳峅 拳痛 岷陳峅 噺 嵳に講詣痛 嵳 貸岫怠エ態岻 結捲喧 犯伐岫なエに岻岾権痛 伐 岷陳峅 権痛 峇 脹 岷陳峅 貸怠 岾権痛 詣痛 伐 岷陳峅 権痛 峇般 (31) where " 岷陳峅 岷陳峅 岷陳峅 (32) 詣痛 噺 茎沈" 掴 岾鶏痛 峇 茎沈" 掴 脹 " 髪 茎沈" 頂 鶏頂 痛貸怠 茎沈" 頂 脹 髪 迎痛 " Where 茎沈 頂 is the Jacobian matrix of 月岫 岻 with respect 岷陳峅 to"航頂 痛貸怠 . We utilize the idea of linear error propagation to derive (23) from (32): " 岷陳峅 岷陳峅 岷陳峅 詣痛 噺 茎沈" 掴 岾鶏痛 峇 茎沈" 掴 脹 " 髪 茎沈" 頂 鶏頂 痛貸怠 茎沈" 頂 脹 髪 迎痛 岷陳峅 噺 茎沈" 掴 岾鶏痛 岷陳峅 峇岾鶏痛 岷陳峅 貸怠 岷陳峅 峇 岾鶏痛 貸怠 脹" Fig. 1. The environment of the simulator. The blue path is the trajectory that robot move along and the green asterisks are the landmarks with 40 Hz control frequency. It also equips a ranger bearing sensor with a なぱどソ frontal view and a maximum range of 30m. The observation frequency is 5 Hz, and the data association is supposed to be known. We evaluate the estimation accuracy of robot pose and the filter consistency among FastSLAM2.0, UFastSLAM and IUFastSLAM for the same environment. 岷陳峅 峇 茎沈" 掴 脹 " 髪 茎沈" 頂 鶏頂 痛貸怠 茎沈" 頂 脹 髪 迎痛 峇 潔剣懸岫捲 券岻脹 髪 " 岶継岷岫傑沈 伐 権岻岫傑沈 伐 権劇髪迎建 (33) As in SUT, 潔剣懸岫捲 券岻 and the right term of (33) are calculated by (14) and (18), respectively. So from (33), (14) and (18), we can get the importance weight calculation: 噺 潔剣懸岫捲 券岻岾鶏痛 岷陳峅 詣痛 岷陳峅 脹 岷陳峅 噺 岾鶏痛 掴津 峇 岾鶏痛 貸怠 岷陳峅 岷陳峅 峇 鶏痛 掴津 " 髪 鶏痛 佃佃 " which is same as discussed in section C. (34) C. Complexity of IUFastSLAM The IUFastSLAM algorithm has a complexity of 頚岫警軽岻, which is same as FastSLAM and UFastSLAM. However, as iteration steps contained in IUFastSLAM, the time cost of IUFastSLAM is larger than UFastSLAM by a constant time factor. A compromise between the computational speed and estimation accuracy has to be made in IUKF, and only applies iteration in path estimation or landmark update step. The other steps are the same as in regular UKF. 1909 Fig. 2. The mean and variance of the robot pose. The base noise is set to 旦 噺 ど ぬ Œエœ 巽 噺 ぬ ソエœ 嘆 噺 ど なŒ" 但 噺 なソ , and the noise level is inflated from 1.5 times to 4 times of base noise. This section evaluates the estimated error of robot pose for each algorithm in different noise levels. The pose error is calculated by the Euclidean distance between the true pose and the estimated pose. Each algorithm executes fifty independent runs with 100 particles. The resample step was performed only after the number of effective particles (Neff) [20] is below 75% of the total number of particles. To increase the simulation speed, only the robot path in first 20 second is used to calculate the estimated error. TABLE I THE COMPARISON OF THREE FSATSLAM ALGORITHMS AT THE BASE NOISE MSE[m] FastSLAM algorthms mean Var FastSLAM2.0 0.27053 0.025650 11 UFastSLAM 0.26260 0.024190 27 IUFastSLAM 0.24581 0.019113 38 Cost[sec] The motion noise is initially set to 旦 噺 ど ぬ" Œエœ " 巽 噺 ぬ ソエœ and the observation noise is set to 嘆 噺 ど なŒ" 但 噺 なソ as the basis for comparison. Then, we inflate both the motion and observation noise levels to several times of the base noise to see the performance of each algorithm at different noise levels. Fig. 2 represents the mean and variance of the estimated pose error for each SLAM algorithms in different noise levels and Table 1 gives the exact mean square error (MSE) result at the base noise level. In Fig. 2 and Table.B, the estimation accuracy of UFastSLAM is better than that of FastSLAM2.0, and the variance of UFastSLAM is also smaller than that of FastSLAM2.0. This is because the Scaled Unscented Transformation (SUT) in UKF produces better estimation than the linear approximation in EKF, which is already verified in [12]. Fig. 2 and TableB also show the proposed IUFastSLAM algorithm gets the smallest MSE and variance. This means the additional iterated step in IUKF further improves the pose estimation accuracy. Therefore, IUFastSLAM is the most stable and accurate one among these solutions. The average time cost of IUFastSLAM is 38s and that of UFastSLAM is 27s running on a Dell personal computer with 2.79 GHZ Dural Core CPU in this simulation, owing to the additional iterated steps in IUFastSLAM. It's worth noting that FastSLAM2.0’s time cost is lower than UFastSLAM and IUFastSLAM. This is because calculating Jacobians in FastSLAM2.0 is a more computational efficient linearization method than the SUT in this simulator. The next experiment is to evaluate how many particles are needed in IUFastSLAM to get the same estimation accuracy when compared to FastSLAM2.0 and UFastSLAM. Fifty independent runs are executed for each algorithm. The robot traveled about 200m in the simulator, and Table C shows the comparison of these algorithms. IUFastSLAM used the least particles to get the same estimation accuracy as others. TABLE C THE COMPARISON OF THREE FSATSLAM ALGORITHMS WITH DIFFERENT NUMBER OF PARTICLES FastSLAM algorthms FastSLAM2.0 UFastSLAM IUFastSLAM Number of particles 80 25 20 MSE[m] 1.0072 1.0124 0.9931 B. Filter Consistency T. Bailey et al. [8] propose the consistency evaluation approach of the RBPF-based FastSLAM algorithms by using 1910 (a) (b) Fig. 3. (a) Consistency of FastSLAM2.0, UFastSLAM and IUFastSLAM with 100 particles. (b) Consistency of FastSLAM2.0, UFastSLAM and IUFastSLAM with 500 particles the average normalized estimation error squared (NEES) as a measure. This method evaluate the RBPF-based algorithms’ particle diversity and degeneration. Fifty Monte Carlo runs were executed and the acceptance interval of the two sided 95% probability concentration region is in [2.36 3.72]. The motion noise is set to 旦 噺 ど ぬ" Œエœ " 巽 噺 ぬ ソエœ and the observation noise is set to 嘆 噺 ど なŒ" 但 噺 なソ . The consistency of first 20 seconds of these algorithms is shown in Fig. 3. IUFastSLAM can prolong a better consistency than FastSLAM2.0 and UFastSLAM in both cases with 100 and 500 particles. The pose estimation accuracy and its uncertainty yield significant impact on the consistency of the RBPF-based FastSLAM solutions. A better pose estimation can maintain more particles around the true pose, and the solution can prolong a consistency for longer time. As particles in IUFastSLAM get more accurate estimation, it has more particles have the accurate information of the state and slow down the loss of particle diversity. C. Car Park dataset Results The Car Park dataset [19] of the University of Sydney is used to evaluate the IUFastSLAM’s performance, and compare it with both UFastSLAM and FastSLAM2.0. Each solution uses 100 particles, and the noise is set to ソ 旦 噺 ど ぱ" Œエœ " 巽 噺 な ぱ ソエœ 嘆 噺 な のŒ" 但 噺 に ぱ . All these solutions take resample step when 軽結血血 隼 のど. The Car Park dataset also contains the GPS data of the robot pose. We Fig. 4. Comparison of the estimated map using FastSLAM2.0, UFastSLAM and IUFastSLAM regard the GPS data as the true path and use this to compare with the estimated path created by these SLAM algorithms. Fig. 4 shows the final maps of these solutions. The blue dashed line shows the true path of robot and the blue circles represent the real pose of beacons. The black solid line is the estimated path and the red asterisks are the estimated pose of beacons in all these maps. From Fig 4, we can see that the estimated paths do not coincide very well with the GPS paths in top right corner in all these solutions, owing to the resample step and the characteristic that RBPF-based FastSLAM solutions can only maintain consistent map in short-term [8]. These solutions lack the strong ability to adjust the landmark estimation since these FastSLAM algorithms do not maintain crossed-covariance between the landmarks due to conditional independence assumption. The result shows that the UFastSLAM gets a better estimated path than that in FastSLAM2.0. Since the UFastSLAM use the UKF to calculate the proposal distribution and the feature update without using the EKF as in FastSLAM2.0, the error propagation in UKF is more accurate than EKF approach. The IUFastSLAM final map coincides very well with the GPS path and the estimated landmarks matches the true beacons as shown in Fig .4. The iterated approach can improve the estimation performance over the UKF one and IUFastSLAM outperforms its counterparts in estimating the map in this dataset. [3] [4] [5] [6] [7] [8] [9] [10] [11] [12] [13] V. CONCLUSION This paper introduces the Iterated Unscented Kalman filter to the FastSLAM framework. The proposed IUFastSLAM algorithm can improve the accuracy of map estimation over the UFastSLAM and FastSLAM2.0 algorithms. IUFastSLAM substitute IUKF for UKF in UFastSLAM and EKF in FastSLAM2.0 in estimating the robot pose and landmark locations. The experimental results demonstrate that the proposed IUFastSLAM algorithm has better performance in estimation accuracy and maintains the filter consistency when compared with previous algorithms. This means it is possible for IUFastSLAM to use less particles in maintaining the estimation accuracy than other solutions. [14] [15] [16] [17] [18] [19] [20] REFERENCES [1] [2] H.Durrant-Whyte and T. Bailey, Simultaneous localization and mapping: Part I, IEEE Robotics and Automation Magazine, vol. 13, no. 2, pp. 99-108, Jun. 2006. 1911 R. Cheeseman and P. Smith, On the representation and estimation of spatial uncertainty, Int. J. Robot. Res, 1986. G. Dissanayake, P. Newman, H.F. Durrant-Whyte, S. Clark, and M.Csobra, A solution to the simultaneous localisation and mapping (SLAM) problem, IEEE Trans. Robot. Automat., vol. 17, no. 3, pp.229–241, 2001. T. Bailey, J. Nieto, J. Guivant, M. Stevens, and E. Nebot, Consistency of the EKF-SLAM algorithm, in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2006, pp. 3562Ｍ3568. K. Murphy, Bayesian map learning in dynamic environments, NIPS-99. M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, Fast-SLAM: A factored solution to the simultaneous localization and mapping problem, Proc. AAAI, 2002. M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, Fast-SLAM2.0: An improved particle filtering algorithm for simultaneous localization and mapping that provably converges, in Proc. Int. Joint Conf. Artif. Intell., 2003, pp. 1151–1156. T. Bailey, J. Nieto, and E. Nebot, Consistency of the FastSLAM algorithm, in Proc. IEEE Int. Conf. Robot. Autom., 2006, pp. 424–429. Julier, Simon J. and Jeffery K. Uhlmann, A New Extension of the Kalman Filter to Nonlinear Systems, In The Proceedings of AeroSense: The 11th International Symposium on Aerospace Defence Sensing, Simulation and Controls, Multi Sensor Fusion, Tracking and Resource Management II, SPIE, 1997. S. Julier and J. K. Uhlmann, Unscented filtering and nonlinear estimation, in Proc. IEEE, vol. 92, no. 3, pp. 401–422, Mar. 2004. R. Martinez-Cantin and J. A. Castellanos, Unscented SLAM for largescale outdoor environments, in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2005, pp. 328–333. Chanki Kim, Rathinasamy Sakthivel and Wan Kyun Chung, Unscented FastSLAM: A Robust and Efficient Solution to the SLAM Problem, IEEE Trans. Robotics, vol. 24, no. 4, pp.808–820, 2008. Khoshnam Shojaie, Kaveh Ahmadi, A. R. Mohammad Shahri. Effects of Iteration in Kalman Filters Family for Improvement of Estimation Accuracy in Simultaneous Localization and Mapping, IEEE/ASME International Conference on Advanced Intelligent Mechatronics, Switzerland, September, 2007. G. Sibley, G. Sukhatme and L. Matthies, The Iterated Sigma Point Kalman Filter with Applications to Long Range Stereo, Proceeding of Robotics: Science and Systems, Philadelphia, USA, 2006. K.Shojaie, A.M.Shahri, Iterated Unscented SLAM Algorithm for Navigation of an Autonomous Mobile Robot, in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2008, pp. 1582-1587. S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics, The MIT Press, 2006. S. Julier, The Scaled Unscented Transformation, Proceedings of the American Control Conference Anchorage, AK May 8-1 0, 2002 Available:http://www-personal.acfr.usyd.edu.au/tbailey/software/inde x.html Available:http://www.acfr.usyd.edu.au/homepages/academic/enebot/d ataset.htm A. Doucet, On sequential simulation-based methods for Bayesian filtering, Technical report, Cambridge University, Department of Engineering, 1998.