# A Strong Tracking SLAM Algorithm Based on the Suboptimal Fading Factor

Authors: Yunpei Liang; Jiahui Dai; Kequan Wang; Xiaobo Li; Pengcheng Xu
Journal: Journal of Sensors
 (2018)

---

## Abstract

This paper proposes an innovative simultaneous localization and mapping (SLAM) algorithm which combines a strong tracking filter (STF), an unscented Kalman filter (UKF), and a particle filter (PF) to deal with the low accuracy of unscented FastSLAM (UFastSLAM). UFastSLAM lacks the capacity for online self-adaptive adjustment, and it is easily influenced by uncertain noise. The new algorithm updates each Sigma point in UFastSLAM by an adaptive algorithm and obtains optimized filter gain by the STF adjustment factor. It restrains the influence of uncertain noise and initial selection. Therefore, the state estimation would converge to the true value rapidly and the accuracy of system state estimation would be improved eventually. The results of simulations and practical tests show that strong tracking unscented FastSLAM (STUFastSLAM) has a significant improvement in accuracy and robustness.


---

## Body

## 1. Introduction

SLAM can solve a problem that a mobile robot can build a global map and estimate its own pose [1, 2] in an unknown environment. The basic principle is to use sensor information and odometer data with multifeature matching to build an environment map while determining the location of the robot in the map. However, because of the complicated external environment, unpredictable system noise, and weak sensor perception problems, the accuracy of SLAM is usually lower than expected [3].Nowadays, many solutions are used to solve the problem of SLAM’s low accuracy. The classic SLAM algorithm is based on an extended Kalman filter (EKF) [4]. In essence, EKF is based on a first-order Taylor formula, and it can solve the problem that the Kalman filter (KF) is not suitable for nonlinear systems [5]. However, there are also some fatal flaws [6], such as its high computational complexity. Moreover, under the condition of high nonlinearity of the system, since EKF is adopted in the SLAM model, there are some problems of lower accuracy, even divergence and nonconvergence. So, based on the EKF-SLAM, some new algorithms have been proposed.Aimed at the disadvantages of computational complexity and the erroneous data associations in EKF-SLAM, the Rao-Blackwellized particle filter was used in the SLAM algorithm by Murphy [7] and Doucet et al. [8]. After that, Montemerlo et al. improved the SLAM algorithm and proposed FastSLAM [9].Based on the FastSLAM algorithm, Kim et al. proposed the UFastSLAM algorithm [10]. The core of the algorithm uses an unscented transform [11, 12] to estimate the transition probability density of SLAM [13]. It cancels the restriction condition of the system model. In other words, the system is not required to be linear. Meanwhile, there is no need to calculate the Jacobian matrix, so the state function and the measurement function are not required to be continuously differentiable. An unscented transform can even be applied to discontinuous systems.UFastSLAM uses an unscented transform to estimate the robot’s pose and uses UKF instead of EKF to estimate the position of environmental features. In practical application, the presence of system noise among relevant information may influence the accuracy of UKF. Meanwhile, the main parameters and the filter gain cannot be adjusted online. It also lacks adaptive ability. In order to improve adaptability and robustness over a rough terrain, we add a strong tracking filter (STF) [14] on the basis of UFastSLAM. STF can adjust filter gain online to achieve the goal of an algorithm self-adapting. STF not only has strong model mismatch robustness but also has the advantages of clear conception and simple calculation. However, because it adds a strong tracking factor, which increases the amount of calculation, the computation time increases slightly with the same number of particles.This paper presents a strong tracking adaptive UFastSLAM (STUFastSLAM) algorithm which is based on the suboptimal fading factor. The new algorithm updates every sigma point by using an adaptive algorithm. It gets the optimized filter gain by utilizing an ST online regulatory factor which can restrain the influence brought by uncertain noise and initial selection on state estimation. STUFastSLAM combines the adaptability of STF with the nonlinear high approximation of UKF. In addition, it also introduces a particle filter which is not influenced by non-Gaussian and nonlinear models. This algorithm has better adaptive ability, accuracy, and robustness [15]. The simulation and experiment demonstrate that the accuracy of the new algorithm is much better than other algorithms in complex environment.


## 2. Related Work

### 2.1. FastSLAM Algorithm Framework

The FastSLAM algorithm combines a particle filter with EKF and develops a new solution of localization and mapping simultaneously. FastSLAM estimates the robot’s pose and environmental features by using PF and EKF. In this way, the computational complexity of FastSLAM is turned intoOM×N. Here, M is the number of particles and N is the features of environment.FastSLAM divides the whole progress of localization and mapping into separate parts. Because the conditions of each environmental landmark are independent, it can also divide the whole mapping progress into mapping on each landmark. If the moving trajectories of the robotXt=x1,x2,…,xt are known and the position of environmental landmarks Mn=m1,m2,…,mn is relatively independent, the joint posterior distribution pXt,M∣zt,ut,nt can be represented as follows:
(1)pXt,M∣zt,ut,nt=pXt∣zt,ut,nt∏i=1Npmi∣Xt,zt,ut,nt,where Zt=z1,z2,…,zt is a sequence of observed data and Ut=u1,u2,…,ut is a sequence of control information. Both of them are used to estimate the moving trajectories of the robot Xt=x1,x2,…,xt, not just its position. nt is the correlation variable.In (1), PF is used to estimate the path and EKF is used to estimate landmarks. The FastSLAM method combines PF and EKF [9].There are two versions of FastSLAM. Based on FastSLAM 1.0, the latest FastSLAM 2.0 introduces observation informationZt. FastSLAM 2.0 improves the accuracy of the algorithm in the process of localization and mapping [16]. However, FastSLAM 2.0 also has some inherent defects. Firstly, FastSLAM 2.0 uses first-order linearization during the process of new position sampling. At this point, it will bring relatively large errors if there is an equation with a high degree of nonlinearity. Secondly, FastSLAM 2.0 uses standard EKF during the process of the landmarks’ observation updating. There are some disadvantages when the FastSLAM 2.0 algorithm deals with equations with higher degrees of nonlinearity. Therefore, in order to solve these problems, some researchers have improved this algorithm under the frame of FastSLAM 2.0.Under the frame of FastSLAM, Grisetti et al. proposed an improved FastSLAM for grid mapping with a Rao-Blackwellized particle filter [17]. This algorithm discretizes the map and uses lasers and odometers to design an important sampling function. However, the accuracy of the grid will affect FastSLAM greatly. In 2009, Arasaratna and Haykin proposed the cubature Kalman filter (CKF) [18]. CKF has a greater performance in regards to nonlinear approximation, numerical accuracy, and filter stability. However, there are some inherent drawbacks of this algorithm. For instance, it has to calculate the tedious covariance matrix square root and reconstruct the covariance matrix during the iteration of SLAM. Kang et al. proposed the neuroevolutionary optimization SLAM (Neo-SLAM) algorithm [19]. Neo-SLAM is a kind of SLAM algorithm which uses neural networks to learn the nonlinear dynamics model and noise statistics data autonomously. Neo-SLAM is insensitive to the changes of system and sensor noise compared with the other algorithms, so it can be used to reduce the robot motion errors [20]. In order to reduce the linearization error of the Kalman filter family, Yadkuri and Khosrowjerdi proposed a slam algorithm based on improved weighted mean extended Kalman filter (IWMEKF) [21].


## 2.1. FastSLAM Algorithm Framework

The FastSLAM algorithm combines a particle filter with EKF and develops a new solution of localization and mapping simultaneously. FastSLAM estimates the robot’s pose and environmental features by using PF and EKF. In this way, the computational complexity of FastSLAM is turned intoOM×N. Here, M is the number of particles and N is the features of environment.FastSLAM divides the whole progress of localization and mapping into separate parts. Because the conditions of each environmental landmark are independent, it can also divide the whole mapping progress into mapping on each landmark. If the moving trajectories of the robotXt=x1,x2,…,xt are known and the position of environmental landmarks Mn=m1,m2,…,mn is relatively independent, the joint posterior distribution pXt,M∣zt,ut,nt can be represented as follows:
(1)pXt,M∣zt,ut,nt=pXt∣zt,ut,nt∏i=1Npmi∣Xt,zt,ut,nt,where Zt=z1,z2,…,zt is a sequence of observed data and Ut=u1,u2,…,ut is a sequence of control information. Both of them are used to estimate the moving trajectories of the robot Xt=x1,x2,…,xt, not just its position. nt is the correlation variable.In (1), PF is used to estimate the path and EKF is used to estimate landmarks. The FastSLAM method combines PF and EKF [9].There are two versions of FastSLAM. Based on FastSLAM 1.0, the latest FastSLAM 2.0 introduces observation informationZt. FastSLAM 2.0 improves the accuracy of the algorithm in the process of localization and mapping [16]. However, FastSLAM 2.0 also has some inherent defects. Firstly, FastSLAM 2.0 uses first-order linearization during the process of new position sampling. At this point, it will bring relatively large errors if there is an equation with a high degree of nonlinearity. Secondly, FastSLAM 2.0 uses standard EKF during the process of the landmarks’ observation updating. There are some disadvantages when the FastSLAM 2.0 algorithm deals with equations with higher degrees of nonlinearity. Therefore, in order to solve these problems, some researchers have improved this algorithm under the frame of FastSLAM 2.0.Under the frame of FastSLAM, Grisetti et al. proposed an improved FastSLAM for grid mapping with a Rao-Blackwellized particle filter [17]. This algorithm discretizes the map and uses lasers and odometers to design an important sampling function. However, the accuracy of the grid will affect FastSLAM greatly. In 2009, Arasaratna and Haykin proposed the cubature Kalman filter (CKF) [18]. CKF has a greater performance in regards to nonlinear approximation, numerical accuracy, and filter stability. However, there are some inherent drawbacks of this algorithm. For instance, it has to calculate the tedious covariance matrix square root and reconstruct the covariance matrix during the iteration of SLAM. Kang et al. proposed the neuroevolutionary optimization SLAM (Neo-SLAM) algorithm [19]. Neo-SLAM is a kind of SLAM algorithm which uses neural networks to learn the nonlinear dynamics model and noise statistics data autonomously. Neo-SLAM is insensitive to the changes of system and sensor noise compared with the other algorithms, so it can be used to reduce the robot motion errors [20]. In order to reduce the linearization error of the Kalman filter family, Yadkuri and Khosrowjerdi proposed a slam algorithm based on improved weighted mean extended Kalman filter (IWMEKF) [21].


## 3. Theoretical Background

### 3.1. Strong Tracking Filter

According to the literature [22], when the theoretical model matches the actual system exactly, the output residual sequence of the KF is nonautocorrelation or a mutually orthogonal Gaussian white noise sequence. However, EKF is a suboptimal filter. It is impossible that the residual sequence is completely uncorrelated. Therefore, as long as the EKF residual sequences are weakly autocorrelated, it can be considered that EKF has good filtering effect.In regards to the STF online real-time adjustment of variable gain matrixKk, the theoretical basis (orthogonality principle) is as follows:Condition 1
(2)Exk−x^kxk−x^kT=min.Condition 2
(3)Eεk+jεkT=0,k=1,2,…,j=1,2,….Condition1 is the performance index of EKF.Condition2 requires that the output residual sequences in different time remain orthogonal.When there is uncertainty in the system model, if the EKF state estimation value deviates from the true state of the system, it will inevitably show up on the mean and magnitude of the output residual sequence. In this case, if the gain matrixKk is adjusted online, condition 2 is forcibly established so that the residual sequences can still be orthogonal to each other and always have Gaussian white noise-like properties. Therefore, when the system model is uncertain, the STF can still keep the capability of effective tracking of status.STF adjusts the gain matrix in real time online and forces the output residual sequence to keep orthogonality by introducing a fading factorλk+1 in the state prediction covariance [22]. Therefore, STF can keep the traceability for the system state even if the system model is uncertain. The fading factor λk+1 is calculated as follows [23]:
(4)λk+1=λ0λ0≥1,1λ0<1,λ0=trNk+1trMk+1,where tr(·)is the operator of trace of the matrix.
(5)Nk+1=Vk+1−Hk+1QkHk+1T−Rk+1,(6)Mk+1=Hk+1Φk+1,kPkΦk+1,kTHk+1T,where Hk+1 is the m × n dimension measurement matrix, Qk is the covariance matrix of control noise, Rk+1 is the covariance matrix of observation noise, and Φk+1,k is the n × n dimension state-transition matrix. Pk is the state prediction covariance.Vk+1 is the covariance matrix of the actual output residual sequence, which is estimated by
(7)Vk+1=ε1ε1T,k=0ρVk+εk+1εk+1T1+ρ,k≥1,where 0<ρ≤1 is the forgetting factor; the value is set to 0.95 usually.The derivation process is shown in the appendix.


## 3.1. Strong Tracking Filter

According to the literature [22], when the theoretical model matches the actual system exactly, the output residual sequence of the KF is nonautocorrelation or a mutually orthogonal Gaussian white noise sequence. However, EKF is a suboptimal filter. It is impossible that the residual sequence is completely uncorrelated. Therefore, as long as the EKF residual sequences are weakly autocorrelated, it can be considered that EKF has good filtering effect.In regards to the STF online real-time adjustment of variable gain matrixKk, the theoretical basis (orthogonality principle) is as follows:Condition 1
(2)Exk−x^kxk−x^kT=min.Condition 2
(3)Eεk+jεkT=0,k=1,2,…,j=1,2,….Condition1 is the performance index of EKF.Condition2 requires that the output residual sequences in different time remain orthogonal.When there is uncertainty in the system model, if the EKF state estimation value deviates from the true state of the system, it will inevitably show up on the mean and magnitude of the output residual sequence. In this case, if the gain matrixKk is adjusted online, condition 2 is forcibly established so that the residual sequences can still be orthogonal to each other and always have Gaussian white noise-like properties. Therefore, when the system model is uncertain, the STF can still keep the capability of effective tracking of status.STF adjusts the gain matrix in real time online and forces the output residual sequence to keep orthogonality by introducing a fading factorλk+1 in the state prediction covariance [22]. Therefore, STF can keep the traceability for the system state even if the system model is uncertain. The fading factor λk+1 is calculated as follows [23]:
(4)λk+1=λ0λ0≥1,1λ0<1,λ0=trNk+1trMk+1,where tr(·)is the operator of trace of the matrix.
(5)Nk+1=Vk+1−Hk+1QkHk+1T−Rk+1,(6)Mk+1=Hk+1Φk+1,kPkΦk+1,kTHk+1T,where Hk+1 is the m × n dimension measurement matrix, Qk is the covariance matrix of control noise, Rk+1 is the covariance matrix of observation noise, and Φk+1,k is the n × n dimension state-transition matrix. Pk is the state prediction covariance.Vk+1 is the covariance matrix of the actual output residual sequence, which is estimated by
(7)Vk+1=ε1ε1T,k=0ρVk+εk+1εk+1T1+ρ,k≥1,where 0<ρ≤1 is the forgetting factor; the value is set to 0.95 usually.The derivation process is shown in the appendix.


## 4. Description of STUFastSLAM

STUFastSLAM is a kind of process which is based on FastSLAM 2.0 as a frame and uses UKF instead of first-order linearization and EKF. Besides, we introduce STF to solve the problem of the poor robustness of UKF when the model is uncertain. The process of STUFastSLAM algorithm is as follows [24]:


### 4.1. Robot State Estimation

At first, the robot’s pose, the control input, and the observation are estimated as a whole. The state matrix after augmentation is(8)xt−1ak=xt−1k00T,Pt−1ak=Pt−1k000Rt000Qt,where xt−1ak is an augmented matrix which contains the robot’s pose, the control input, and the observation, and its dimension is 7 × 1. Pt−1ak is an augmented covariance matrix which contains robot covariance, control noise, and observation noise, and its dimension is 7 × 7. Rt and Qt are the observation noise covariance and control noise covariance, respectively [25].Step 1.
2N + 1 sigma points are extracted around the mean point deterministically. N is the dimension of the mean vector. Here N=7. So, the set of extracted sigma points are [26]
(9)χt−1a0k=xt−1ak,(10)χt−1aik=xt−1ak+N+λPt−1akii=1,…,N,(11)χt−1aik=xt−1ak−N+λPt−1aki−Ni=1,…,2N,where, at this moment, each sigma point contains robot state, control input, and observation information, so that we can calculate the robot state information by the sigma point which is transformed by the nonlinear kinematic model. We can get the robot state information as follows.Step 2.
Get the prediction equation(12)χ¯tik=fut,χt−1ik.The robot state mean and the prediction of the covariance can be calculated as follows:(13)xt∣t−1k=∑i=02Nωmiχ¯tik,(14)Pt∣t−1kl=∑i=02Nωciχ¯tik−xt∣t−1kχ¯tik−xt∣t−1kT.In order to make the filter more robust when the system model is uncertain, the fading factorλt is introduced into the state prediction covariance matrix. Pt∣t−1k is calculated as follows:
(15)Pt∣t−1k=λt∑i=02Nωciχ¯tik−xt∣t−1kχ¯tik−xt∣t−1kT.Hereλt is unknown; we will calculate the value of λt in the latter part.ωmi is used for calculating the weight of the mean. ωci is used for calculating the weight of the covariance. The formulas are as follows.
(16)ωm0=λn+λ,(17)ωc0=λn+λ+1−α2+β,(18)ωci=ωmi=12n+λfori=1,…,2n,where β=2. In order to obtain more accuracy on the robot’s state, observation information must be considered. We update the prediction by using the observed features. The formulas are as follows.
(19)N¯tikl=hχ¯tikl,μk^,t−1k,(20)n^tkl=∑i=02NωmiN¯tikl,(21)Stkl=∑i=02NωciN¯tikl−n^tkN¯tikl−n^tkT.Utilizingxt∣t−1k and Pt∣t−1kl, the sigma point is calculated according to the sampling method selected by step 1. Based on the nonlinear function and (19), (20), and (21), we can get the sigma point N¯tikl. From N¯tikl, we can get the output prediction n^tkl, the autocovariance Stkl, and the cross covariance Σtx,nkl.N¯tikl is the sigma point which is calculated via the nonlinear observation equation. n^tkl is the prediction of observation, and Stkl is the updating vector (autocovariance). Thus, the cross covariance matrix Σtx,nkl can be obtained.
(22)∑tx,nkl=∑i=02Nωciχ¯tikl−xt∣t−1kN¯tikl−n^tkT.Pt∣t−1kl, Stkl, and Σtx,nkl are known. The fading factor λt in (15) can be calculated by (4), (5), (6), and (7).The fading factorλt has already been obtained. So we use xt∣t−1k and Pt∣t−1k to calculate the sigma point χt−1aik in accordance with the sample strategy in step 1. The sigma point χt−1aik transforms into N¯tik through the nonlinear measurement function. We can calculate the output prediction n^tk, the autocovariance Stk, and the cross-covariance Σtx,nk from N¯tik.
(23)N¯tik=hχ¯tik,μk^,t−1k,(24)n^tk=∑i=02NωmiN¯tik,(25)Stk=∑i=02NωciN¯tik−n^tkN¯tik−n^tkT,(26)∑tx,nk=∑i=02Nωciχ¯tik−xt∣t−1kN¯tik−n^tkT.Finally, the Kalman gainKtk can be calculated as follows:
(27)Ktk=∑tx,nkStk−1.At timet, the robot state mean xtk and the covariance estimation Ptk can be obtained as follows:
(28)xtk=xt∣t−1k+Ktkzt−n^tk,(29)Ptk=Pt∣t−1k−KtkStkKtkT.We update the set of sigma points in (10) based on (28) and (29). The new sigma points set are as follows:
(30)χtaik=xtak,xtak±N+λPtak.


### 4.2. Environmental Feature Position Estimation

We then have to estimate and update the observed environmental features. Similar with the robot’s pose estimation, we need to structure the sigma points set before we estimate the environmental features. As the SLAM is in 2D scene, the dimension of the environmental feature is 2. The sigma points are as follows:(31)xa0k=μnt,t−1k,xaik=μnt,t−1k+n+λ∑nt,t−1kii=1,…,n,xaik=μnt,t−1k−n+λ∑nt,t−1ki−ni=1,…,2n,where μnt,t−1k is the mean of the nth feature at t−1, and Σnt,t−1k is its covariance. The other parameters can be calculated like in the previous section. Then, we can get
(32)N¯tik=hχik,xtki=0,…,2n,z^tk=∑i=02NωmiZ¯tik,S¯tk=∑i=02NωciZ¯tik−z^tkZ¯tik−n¯tkT.Here,N¯tik is the sigma point set which is calculated by nonlinear observation formulas. z^tk and S¯tk are the predicted mean and covariance of observation, respectively. Then, we can calculate the Kalman gain K¯tk and update the mean μnt,tk and covariance Σnt,tk of the environmental feature.
(33)Σ¯tk=∑i=02NωciXik−μnt,t−1kZ¯tik−z^tkT,(34)K¯tk=Σ¯tkS¯tk−1,(35)μnt,tk=μnt,t−1k+K¯tkzt−z^tk,(36)∑nt,tk=∑nt,t−1k−K¯tkS¯tkK¯tkT.


### 4.3. Resampling Strategy

In the last step, we have to make the judgment resample for the particles. The resampling method is exactly the same as that of FastSLAM 2.0.(37)wtk=targetdistributionproposaldistribution=2πLtk−1/2exp−12zt−z¯tLtk−1zt−z¯t.Here,(38)Ltk=∑tx,nkTPtk−1∑tx,nk+S¯tk.


## 4.1. Robot State Estimation

At first, the robot’s pose, the control input, and the observation are estimated as a whole. The state matrix after augmentation is(8)xt−1ak=xt−1k00T,Pt−1ak=Pt−1k000Rt000Qt,where xt−1ak is an augmented matrix which contains the robot’s pose, the control input, and the observation, and its dimension is 7 × 1. Pt−1ak is an augmented covariance matrix which contains robot covariance, control noise, and observation noise, and its dimension is 7 × 7. Rt and Qt are the observation noise covariance and control noise covariance, respectively [25].Step 1.
2N + 1 sigma points are extracted around the mean point deterministically. N is the dimension of the mean vector. Here N=7. So, the set of extracted sigma points are [26]
(9)χt−1a0k=xt−1ak,(10)χt−1aik=xt−1ak+N+λPt−1akii=1,…,N,(11)χt−1aik=xt−1ak−N+λPt−1aki−Ni=1,…,2N,where, at this moment, each sigma point contains robot state, control input, and observation information, so that we can calculate the robot state information by the sigma point which is transformed by the nonlinear kinematic model. We can get the robot state information as follows.Step 2.
Get the prediction equation(12)χ¯tik=fut,χt−1ik.The robot state mean and the prediction of the covariance can be calculated as follows:(13)xt∣t−1k=∑i=02Nωmiχ¯tik,(14)Pt∣t−1kl=∑i=02Nωciχ¯tik−xt∣t−1kχ¯tik−xt∣t−1kT.In order to make the filter more robust when the system model is uncertain, the fading factorλt is introduced into the state prediction covariance matrix. Pt∣t−1k is calculated as follows:
(15)Pt∣t−1k=λt∑i=02Nωciχ¯tik−xt∣t−1kχ¯tik−xt∣t−1kT.Hereλt is unknown; we will calculate the value of λt in the latter part.ωmi is used for calculating the weight of the mean. ωci is used for calculating the weight of the covariance. The formulas are as follows.
(16)ωm0=λn+λ,(17)ωc0=λn+λ+1−α2+β,(18)ωci=ωmi=12n+λfori=1,…,2n,where β=2. In order to obtain more accuracy on the robot’s state, observation information must be considered. We update the prediction by using the observed features. The formulas are as follows.
(19)N¯tikl=hχ¯tikl,μk^,t−1k,(20)n^tkl=∑i=02NωmiN¯tikl,(21)Stkl=∑i=02NωciN¯tikl−n^tkN¯tikl−n^tkT.Utilizingxt∣t−1k and Pt∣t−1kl, the sigma point is calculated according to the sampling method selected by step 1. Based on the nonlinear function and (19), (20), and (21), we can get the sigma point N¯tikl. From N¯tikl, we can get the output prediction n^tkl, the autocovariance Stkl, and the cross covariance Σtx,nkl.N¯tikl is the sigma point which is calculated via the nonlinear observation equation. n^tkl is the prediction of observation, and Stkl is the updating vector (autocovariance). Thus, the cross covariance matrix Σtx,nkl can be obtained.
(22)∑tx,nkl=∑i=02Nωciχ¯tikl−xt∣t−1kN¯tikl−n^tkT.Pt∣t−1kl, Stkl, and Σtx,nkl are known. The fading factor λt in (15) can be calculated by (4), (5), (6), and (7).The fading factorλt has already been obtained. So we use xt∣t−1k and Pt∣t−1k to calculate the sigma point χt−1aik in accordance with the sample strategy in step 1. The sigma point χt−1aik transforms into N¯tik through the nonlinear measurement function. We can calculate the output prediction n^tk, the autocovariance Stk, and the cross-covariance Σtx,nk from N¯tik.
(23)N¯tik=hχ¯tik,μk^,t−1k,(24)n^tk=∑i=02NωmiN¯tik,(25)Stk=∑i=02NωciN¯tik−n^tkN¯tik−n^tkT,(26)∑tx,nk=∑i=02Nωciχ¯tik−xt∣t−1kN¯tik−n^tkT.Finally, the Kalman gainKtk can be calculated as follows:
(27)Ktk=∑tx,nkStk−1.At timet, the robot state mean xtk and the covariance estimation Ptk can be obtained as follows:
(28)xtk=xt∣t−1k+Ktkzt−n^tk,(29)Ptk=Pt∣t−1k−KtkStkKtkT.We update the set of sigma points in (10) based on (28) and (29). The new sigma points set are as follows:
(30)χtaik=xtak,xtak±N+λPtak.


## 4.2. Environmental Feature Position Estimation

We then have to estimate and update the observed environmental features. Similar with the robot’s pose estimation, we need to structure the sigma points set before we estimate the environmental features. As the SLAM is in 2D scene, the dimension of the environmental feature is 2. The sigma points are as follows:(31)xa0k=μnt,t−1k,xaik=μnt,t−1k+n+λ∑nt,t−1kii=1,…,n,xaik=μnt,t−1k−n+λ∑nt,t−1ki−ni=1,…,2n,where μnt,t−1k is the mean of the nth feature at t−1, and Σnt,t−1k is its covariance. The other parameters can be calculated like in the previous section. Then, we can get
(32)N¯tik=hχik,xtki=0,…,2n,z^tk=∑i=02NωmiZ¯tik,S¯tk=∑i=02NωciZ¯tik−z^tkZ¯tik−n¯tkT.Here,N¯tik is the sigma point set which is calculated by nonlinear observation formulas. z^tk and S¯tk are the predicted mean and covariance of observation, respectively. Then, we can calculate the Kalman gain K¯tk and update the mean μnt,tk and covariance Σnt,tk of the environmental feature.
(33)Σ¯tk=∑i=02NωciXik−μnt,t−1kZ¯tik−z^tkT,(34)K¯tk=Σ¯tkS¯tk−1,(35)μnt,tk=μnt,t−1k+K¯tkzt−z^tk,(36)∑nt,tk=∑nt,t−1k−K¯tkS¯tkK¯tkT.


## 4.3. Resampling Strategy

In the last step, we have to make the judgment resample for the particles. The resampling method is exactly the same as that of FastSLAM 2.0.(37)wtk=targetdistributionproposaldistribution=2πLtk−1/2exp−12zt−z¯tLtk−1zt−z¯t.Here,(38)Ltk=∑tx,nkTPtk−1∑tx,nk+S¯tk.


## 5. STUFastSLAM Overview

According to Section4, the STUFastSLAM algorithm is composed of two parts. One is the estimation of robot pose, and the other is the estimation of the location of environmental features. We drew flow charts for these two parts separately. The flow charts of the algorithm are as follows:Figure1 is the flow chart of robot state estimation. It is divided into two parts: prediction and update.Figure 1
Flow chart of robot state estimation.The flow chart of environmental feature position estimation is shown as Figure2.Figure 2
Flow chart of environmental feature position estimation.The explanation of the STUFastSLAM algorithm is in the pseudocode. The pseudocode of the STUFastSLAM algorithm is as follows.Pseudocode 1: STUFastSLAM algorithm.
Initialization parameters
for 𝑘 = 1 𝑡𝑜 𝑀
% Robot state estimation
Extract the robot positionxt−1k from sigma points set 𝑋𝑡−1 (10)
Predict meanxt∣t−1k (13) and covariance Pt∣t−1kl (14) of robot
Associate observation information data
% The calculation of fading factor
Calculate the fading factorλk+1 (4) from the prediction of the covariance Pt∣t−1kl, the autocovariance Stkl (21) and the cross covariance Σtx,nkl (22).
% Introduce the fading factorλt
Obtain the predicted covariance of the robot after the introduction fading factor (15)
Obtain the autocovarianceStk (25) and the cross-covariance Σtx,nk (26) of the robot after the introduction fading factor
fork^=known feature
Update meanxtk (28) and covariance Ptk (29) of the robot
Update sigma points (30)
Calculate importance weight (17)
end for
% Environmental features position estimation
ifk^=new feature
Initialize new feature meanμnt,t−1k and covariance Σnt,t−1k
else
Update mean (35) and covariance (36) of features
end if
for unobserved features
μnt,tk=μnt,t−1k, Σnt,tk=Σnt,t−1k
end for
Add updated {xtk, Ntk, μNtk,tk, ΣNtk,tk} points set Xaux
end for
% Resampling strategy
for 𝑘 = 1 𝑡𝑜 𝑀
Normalize weight and calculateωneff (37)
ifωneff<ωλ
Resample
else
Maintain the original particle weight
end for
Add new particles toXt
ReturnXt.The covariancePt∣t−1kl, the autocovariance Stkl, and the cross covariance ∑tx,nkl indicate that the fading factor has not been introduced.The covariancePt∣t−1k, the autocovariance, and the cross-covariance indicate that the fading factor has been introduced.


## 6. Simulation Result

In order to verify the reliability and validity of STUFastSLAM, we designed a simulation experiment and compared the results among STUFastSLAM, FastSLAM 2.0, and UFastSLAM. The simulation experiment was carried out on the Matlab2015 platform. The SLAM simulator was published by the University of Sydney robot center [27]. We use the car-like motion model, and the observation model is the Bearing-Rang radius scan model. The wheel base is 4 m, and the robot velocity is V=3m/s. The effective range of sensor is 40 m, and its front view is 180°. The noise is (0,1) distributional Gaussian white noise. The process noise is (σV=0.3m/s, σG=3°), and the observation noise is (σr=0.4m, σθ=5°).Figures3 and 4 are simulation environment graphs. The green “※”’s are the environmental features, and the “O”’s are the waypoints of the robot’s moving path. The curve is the robot’s reference path. The robot moves from the point of origin and follows the reference path for 2 laps. In this experiment, we set 35 features and 17 waypoints. The robot starts from (0,0) point and moves along the path (Figure 3). All the tests were repeated 30 times, and we obtained the average value as the statistical results.Figure 3
Simulation environment.Figure 4
Robot path graph.In Figure4, the black curve is the estimated path trajectory of the robot. The red points show the estimated landmarks. Because of odometer and observation errors, there is some deviation between the estimated and true paths and between estimated and true landmarks. In the simulation experiment, we compare the true path with the estimated path of different algorithms and compare the location deviation of the true landmarks with the estimated ones to compare the accuracy of the algorithms. The smaller the deviation, the more accurately the robot draws the map. By comparing the time of the car run the whole distance, we can compare the calculation speed of the algorithms. The shorter the time, the lower the computational complexity of the algorithm, and the simpler the calculation.Figures5–7 show the comparison of the simulation path of FastSLAM 2.0, UFastSLAM, and STUFastSLAM. In the beginning, the estimation accuracy of each of these 3 algorithms is high. The estimated path is basically consistent with the true path, and the deviation between them is very small. With the increase of robot walking distance, there is some deviation between the estimated path and the true path. We find that the deviation of FastSLAM 2.0 is the greatest. The accuracy of UFastSLAM is higher than that of FastSLAM 2.0 in the latter period. Due to the addition of adaptive factors, STUFastSLAM not only has greatly improved the estimation accuracy but also can keep high position estimation in the process of robot exploration. The coincidence degree of the estimated path and the true path is the highest.Figure 5
FastSLAM 2.0 simulation graph.Figure 6
UFastSLAM 2.0 simulation graph.Figure 7
STUFastSLAM simulation graph.Figure8 is the comparison of the position estimation error among standard FastSLAM 2.0, UFastSLAM, and STUFastSLAM. The system noise and the observation noise are in the condition of Gauss white noise. The x-axis presents the number of steps the experiment took, and each experiment takes 17,383 steps. The first lap is 1~8691 steps, and the second lap is 8692~17,383 steps. Firstly, the estimation accuracy is significantly improved in the second lap in Figure 8. This is because when the robot runs the second lap, it matches and corrects the path of the first lap. Secondly, the accuracy of FastSLAM 2.0 is lower than that of the other two algorithms, and the maximum location error is higher than in the other two. In STUFastSLAM, the introduction of an adaptive factor causes the error in the simulation state to be well controlled. In most cases, the error of STUFastSLAM is less than those of FastSLAM 2.0 and UFastSLAM. Especially in the simulation of the second lap, the error during the overall process is in the minimum state, which is consistent with the previous theoretical analysis.Figure 8
Estimated position error in different algorithms.In order to verify the accuracy and stability of the three algorithms in different levels of environmental noise, we set five groups of environmental noise: (σr=0.1m, σb=3°), (σr=0.3m, σb=3°), (σr=0.4m, σb=5°), and (σr = 0.4 m, σb = 8°). The particle number is set to 10, and the odometer noise is set to (σv=0.3m/s, σG=3°). We use the root mean square error (RMSE) to estimate the deviation between the observation value and the true value. RMSE is defined as
(39)RMSE=∑k=0Nsxk−x^k2Ns−11/2where xk is the true state of the robot and x^k is the estimated state at time k.From Figure9, we can find that the RMSE of these three algorithms rise to different extents as the noise grows, which represents their accuracy decreasing. But in all the cases, the RMSE of STUFastSLAM is minimal. When the noise is σr=0.1, σb=3, the RMSE of UFastSLAM is very close to that of FastSLAM, and the RMSE of STUFastSLAM is significantly lower. With the increase in the noise level, the advantage of STUFastSLAM becomes more and more clear and the gap between the RMSE of STUFastSLAM and those of the other two algorithms grows. When the noise is σr=0.4, σb=8, the gap between the RMSEs of UFastSLAM and STUFastSLAM becomes small, but the gap with FastSLAM 2.0 is even more evident.Figure 9
RMSE of different algorithms in different levels of measurement noise.Figure10 shows the computation time of these three algorithms under the same RMSE. From Figure 10, the computation time of STUFastSLAM is least, and the computation time of UFastSLAM is slightly more than that of STUFastSLAM. The computation time of FastSLAM 2.0 is far greater than those of the other two. It indicates that STUFastSLAM still has the time advantage compared with the other two algorithms under the same accuracy.Figure 10
Computation time under the same RMSE.Figure11 shows the comparison of RMSE in the case of different quantities of particles when STUFastSLAM has the same noise level. When the number of particles is less than or equal to 20, the RMSE value of this algorithm shows a linear downtrend basically. When the number of particles is greater than 20, the decrease in the RMSE value becomes gentler. It indicates that the accuracy of this algorithm is gradually improved with the increase in particle number. However, when the number of particles is greater than 20, the increase in accuracy is not obvious. Meanwhile, a great deal of computation time is also needed. Therefore, the STUFastSLAM algorithm essentially achieves its optimal level when the number of particles is 20.Figure 11
RMSE of STUFastSLAM by different numbers of particles.Figure12 shows the position estimation error of STUFastSLAM in the case of different numbers of particles. Figure 12 corresponds to Figure 11; when the number of particles is between 5 and 20, the estimated accuracy improves obviously as the number of particles increases. When the number of particles is 40, the accuracy is only slightly higher than that of 20 particles.Figure 12
Estimation position error of STUFastSLAM in different numbers of particles.


## 7. Experiment Base on P3DX Robot

In order to verify the actual performance of the algorithm, we simulated robot real-time localization and mapping under rough road conditions in the office.This experimental platform adopts the Pioneer-3DX (P3DX) autonomous mobile robot which is produced by American Active Media Company, as shown in Figure13. It is a small intelligent robot driven by two wheels with differential drive. It has many advantages, such as flexibility, stability, generality, and easy operation. P3DX is the most mature experimental platform for the wheeled mobile robots in the world.Figure 13
P3DX robot.


### 7.1. Experiment 1

Figure14 is a real photo of our office. The experiment will build a 2D environment map of the office in this condition. In order to verify the SLAM performance under rough road conditions, we threw some gravel and sand on the ground to simulate the condition of rough roads and introduce skidding.Figure 14
Experiment environment.In order to test the performance of the algorithm quantitatively, we used the laser rangefinder to measure the office and drew the following environment map with SolidWorks as shown in Figure15.Figure 15
Office room environment CAD graph.We chose 5 points A, B, C, D, and E in Figure16 as the distance control points of indoor environment which can be used to compare the accuracy of algorithms. The dimensions are as shown in Table 1:Figure 16
Office floor plan.Table 1
Control point dimension.
Control points
Dimension (mm)
A–B
12,345
A–C
4979
A–D
5300
A–E
7825We compared the performance with UFastSLAM and STUFastSLAM in the same test experiment conditions. We controlled the robot to walk in the office to build an office map.Figure17 is the environment model which was built by UFastSLAM. Figure 17 is the somewhat distorted map due to the robot’s wheels skidding. The map is too vague to show the real environment of the office.Figure 17
Indoor environment scanning chart (UFastSLAM).Figure18 is the environment model which was built by STUFastSLAM. As the reader can see in Figure 18, the map is much clearer than in Figure 17. In this map, the corners can be seen clearly and the wall is vertical. The entire map is seen in much sharper detail.Figure 18
Indoor environment scanning chart (STUFastSLAM).In order to quantify the gap between the two algorithms, we built the office map 10 times to obtain the average dimensions of the control points as follows:Table2 shows the average dimensions of the control points obtained by STUFastSLAM.Table 2
STUFastSLAM control point dimension.
Control points
Dimension (mm)
A–B
12,234
A–C
4872
A–D
5431
A–E
7715Table3 shows the average dimensions of the control points obtained by UFastSLAM.Table 3
UFastSLAM control point dimension.
Control points
Dimension (mm)
A–B
11,873
A–C
4869
A–D
5325
A–E
7705The resulting analysis table is shown below:From Figure19, we can see that in the same conditions, the average error of STUFastSLAM is much lower than the one of UFastSLAM when mapping an indoor environment.Figure 19
Average error of different algorithms in a real condition.Figure20 shows the RMSE of different algorithms in a real environment. We can see that the RMSE of STUFastSLAM is much lower than that of UFastSLAM.Figure 20
RMSE of different algorithms in a real condition.


### 7.2. Experiment 2

In this experiment, we let the robot go from point A, through gate B, walk in a 13.36 m long, 4.77 m wide hut, and then walk out of gate C and enter a 2.26 m wide corridor and return to the starting point (as shown in Figure21). The road was covered with gravel to simulate the situation of the robot walking on harsh road conditions. The accuracy of the drawn map is judged by comparing the map size measured by the slam algorithm with the actual layout of the building.Figure 21
Robot indoor walking track.We used the STUFastSLAM algorithm and the UFastSLAM algorithm to create maps and compare the two maps.In Figure22, the red map is drawn by the STUFastSLAM algorithm, and the gray map is drawn by the UFastSLAM algorithm. As can be seen from the figure, the red map is obviously more regular than the gray map and can reflect various indoor details not mapped by UFastSLAM. In order to quantitatively compare the accuracy of the two algorithms, the distances between the points in the map are measured and compared with the actual measurements of the building. We got Figure 23.Figure 22
Comparison of maps drawn by STUFastSLAM and UFastSLAM.Figure 23
Comparison of error in STUFastSLAM and UFastSLAM.From Figure23, the error of STUFastSLAM is far lower than that of UFastSLAM in terms of maximum error, minimum error, average error, and standard deviation.Figure24 shows the RMSE of different algorithms in a real environment. We can see that the RMSE of STUFastSLAM is much lower than that of UFastSLAM. The RMSE value is slightly different from that of Figure 20 due to the differences in the road conditions and the indoor space. It still, however, certifies that the new algorithm can obtain better performance in practical applications.Figure 24
RMSE of different algorithms in a real condition.To verify the effect of the number of particles on map accuracy, we set the number of particles in the STUFastSLAM algorithm to be 10, 20, and 40 particles, respectively.As can be seen from Figure25, as the number of particles increases, the quality of the map drawn by the robot progressively improves. We measured the size of the three maps and compared them with the actual maps, as shown in Figure 26.Figure 25
Comparison of maps with different particle numbers.Figure 26
RMSE of STUFastSLAM by different numbers of particles.Figure26 and Figure 11 provide confirmation for one another. As the number of particles increases, the RMSE value of the map decreases, and thus the accuracy of the map is increased.


## 7.1. Experiment 1

Figure14 is a real photo of our office. The experiment will build a 2D environment map of the office in this condition. In order to verify the SLAM performance under rough road conditions, we threw some gravel and sand on the ground to simulate the condition of rough roads and introduce skidding.Figure 14
Experiment environment.In order to test the performance of the algorithm quantitatively, we used the laser rangefinder to measure the office and drew the following environment map with SolidWorks as shown in Figure15.Figure 15
Office room environment CAD graph.We chose 5 points A, B, C, D, and E in Figure16 as the distance control points of indoor environment which can be used to compare the accuracy of algorithms. The dimensions are as shown in Table 1:Figure 16
Office floor plan.Table 1
Control point dimension.
Control points
Dimension (mm)
A–B
12,345
A–C
4979
A–D
5300
A–E
7825We compared the performance with UFastSLAM and STUFastSLAM in the same test experiment conditions. We controlled the robot to walk in the office to build an office map.Figure17 is the environment model which was built by UFastSLAM. Figure 17 is the somewhat distorted map due to the robot’s wheels skidding. The map is too vague to show the real environment of the office.Figure 17
Indoor environment scanning chart (UFastSLAM).Figure18 is the environment model which was built by STUFastSLAM. As the reader can see in Figure 18, the map is much clearer than in Figure 17. In this map, the corners can be seen clearly and the wall is vertical. The entire map is seen in much sharper detail.Figure 18
Indoor environment scanning chart (STUFastSLAM).In order to quantify the gap between the two algorithms, we built the office map 10 times to obtain the average dimensions of the control points as follows:Table2 shows the average dimensions of the control points obtained by STUFastSLAM.Table 2
STUFastSLAM control point dimension.
Control points
Dimension (mm)
A–B
12,234
A–C
4872
A–D
5431
A–E
7715Table3 shows the average dimensions of the control points obtained by UFastSLAM.Table 3
UFastSLAM control point dimension.
Control points
Dimension (mm)
A–B
11,873
A–C
4869
A–D
5325
A–E
7705The resulting analysis table is shown below:From Figure19, we can see that in the same conditions, the average error of STUFastSLAM is much lower than the one of UFastSLAM when mapping an indoor environment.Figure 19
Average error of different algorithms in a real condition.Figure20 shows the RMSE of different algorithms in a real environment. We can see that the RMSE of STUFastSLAM is much lower than that of UFastSLAM.Figure 20
RMSE of different algorithms in a real condition.


## 7.2. Experiment 2

In this experiment, we let the robot go from point A, through gate B, walk in a 13.36 m long, 4.77 m wide hut, and then walk out of gate C and enter a 2.26 m wide corridor and return to the starting point (as shown in Figure21). The road was covered with gravel to simulate the situation of the robot walking on harsh road conditions. The accuracy of the drawn map is judged by comparing the map size measured by the slam algorithm with the actual layout of the building.Figure 21
Robot indoor walking track.We used the STUFastSLAM algorithm and the UFastSLAM algorithm to create maps and compare the two maps.In Figure22, the red map is drawn by the STUFastSLAM algorithm, and the gray map is drawn by the UFastSLAM algorithm. As can be seen from the figure, the red map is obviously more regular than the gray map and can reflect various indoor details not mapped by UFastSLAM. In order to quantitatively compare the accuracy of the two algorithms, the distances between the points in the map are measured and compared with the actual measurements of the building. We got Figure 23.Figure 22
Comparison of maps drawn by STUFastSLAM and UFastSLAM.Figure 23
Comparison of error in STUFastSLAM and UFastSLAM.From Figure23, the error of STUFastSLAM is far lower than that of UFastSLAM in terms of maximum error, minimum error, average error, and standard deviation.Figure24 shows the RMSE of different algorithms in a real environment. We can see that the RMSE of STUFastSLAM is much lower than that of UFastSLAM. The RMSE value is slightly different from that of Figure 20 due to the differences in the road conditions and the indoor space. It still, however, certifies that the new algorithm can obtain better performance in practical applications.Figure 24
RMSE of different algorithms in a real condition.To verify the effect of the number of particles on map accuracy, we set the number of particles in the STUFastSLAM algorithm to be 10, 20, and 40 particles, respectively.As can be seen from Figure25, as the number of particles increases, the quality of the map drawn by the robot progressively improves. We measured the size of the three maps and compared them with the actual maps, as shown in Figure 26.Figure 25
Comparison of maps with different particle numbers.Figure 26
RMSE of STUFastSLAM by different numbers of particles.Figure26 and Figure 11 provide confirmation for one another. As the number of particles increases, the RMSE value of the map decreases, and thus the accuracy of the map is increased.


## 8. Conclusions

This article puts forth a solution for the problems of rescue robots’ poor mapping accuracy and mapping maps easily diverging in complex terrain environments in the form of the STUFastSLAM algorithm. This article is based on the framework of UFastSLAM and innovatively introduces the strong tracking adaptive factor. For each sigma point in the UFastSLAM algorithm, adaptive algorithms are used to update them in real time. Optimized filter gain is obtained through the STF online adjustment factor, and the influence of uncertain noise and initial value selection on the state estimation is suppressed. By this, system state estimation accuracy is increased. The robustness of the system is enhanced, so the new algorithm is more suitable for environments with harsh terrain. In addition, we carried out simulation experiments to verify the effectiveness of the algorithm. The experimental results show that the accuracy and stability of STUFastSLAM are better than those of the other two algorithms under different noise conditions. Even under the same accuracy, it has less computation time than the other two algorithms have. Finally, in the office, we simulated robot real-time localization and mapping under rough road conditions. The results show that the map drawn by the rescue robot much better reflects the surrounding environment, and the map has high accuracy and good reliability.Through a series of simulations and experiments, the performance of STUFastSLAM is verified. The algorithm is superior to other similar algorithms in accuracy and reliability. It can successfully accomplish the task of mapping in complex terrain environments.



---

*Source: 9684382-2018-05-03.xml*