Journal of Sensors

Volume 2018, Article ID 9684382, 14 pages

https://doi.org/10.1155/2018/9684382

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

^{1}State Key Laboratory of Coal Mine Disaster Dynamics and Control, Chongqing University, Chongqing 400044, China^{2}China Coal Technology Engineering Group Chongqing Research Institute, Chongqing 400037, China

Correspondence should be addressed to Jiahui Dai; moc.361@123-321-jjw

Received 17 August 2017; Revised 2 March 2018; Accepted 19 March 2018; Published 3 May 2018

Academic Editor: Pietro Siciliano

Copyright © 2018 Yunpei Liang et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

#### 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.

#### 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 into . 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 robot are known and the position of environmental landmarks is relatively independent, the joint posterior distribution can be represented as follows: where is a sequence of observed data and is a sequence of control information. Both of them are used to estimate the moving trajectories of the robot , not just its position. 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 information 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 matrix , the theoretical basis (orthogonality principle) is as follows:

*Condition 1 *

*Condition 2 *

Condition 1 is the performance index of EKF.

Condition 2 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 matrix 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 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 is calculated as follows [23]:
where tr(·)is the operator of trace of the matrix.
where is the *m* × *n* dimension measurement matrix, is the covariance matrix of control noise, is the covariance matrix of observation noise, and is the *n* × *n* dimension state-transition matrix. is the state prediction covariance.

is the covariance matrix of the actual output residual sequence, which is estimated by where 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 where is an augmented matrix which contains the robot’s pose, the control input, and the observation, and its dimension is 7 × 1. is an augmented covariance matrix which contains robot covariance, control noise, and observation noise, and its dimension is 7 × 7. and are the observation noise covariance and control noise covariance, respectively [25].

*Step 1. *2*N* + 1 sigma points are extracted around the mean point deterministically. *N* is the dimension of the mean vector. Here . So, the set of extracted sigma points are [26]
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

The robot state mean and the prediction of the covariance can be calculated as follows:

In order to make the filter more robust when the system model is uncertain, the fading factor is introduced into the state prediction covariance matrix. is calculated as follows:

Here is unknown; we will calculate the value of in the latter part.

is used for calculating the weight of the mean. is used for calculating the weight of the covariance. The formulas are as follows. where . 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.

Utilizing and , 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 . From , we can get the output prediction , the autocovariance , and the cross covariance .

is the sigma point which is calculated via the nonlinear observation equation. is the prediction of observation, and is the updating vector (autocovariance). Thus, the cross covariance matrix can be obtained.

, , and are known. The fading factor in (15) can be calculated by (4), (5), (6), and (7).

The fading factor *λ _{t}* has already been obtained. So we use and to calculate the sigma point in accordance with the sample strategy in step 1. The sigma point transforms into through the nonlinear measurement function. We can calculate the output prediction , the autocovariance , and the cross-covariance from .

Finally, the Kalman gain can be calculated as follows:

At time *t*, the robot state mean and the covariance estimation can be obtained as follows:

We update the set of sigma points in (10) based on (28) and (29). The new sigma points set are as follows:

##### 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:
where is the mean of the *n*th feature at , and is its covariance. The other parameters can be calculated like in the previous section. Then, we can get

Here, is the sigma point set which is calculated by nonlinear observation formulas. and are the predicted mean and covariance of observation, respectively. Then, we can calculate the Kalman gain and update the mean and covariance of the environmental feature.

##### 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.

Here,

#### 5. STUFastSLAM Overview

According to Section 4, 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:

Figure 1 is the flow chart of robot state estimation. It is divided into two parts: prediction and update.