A FastSLAM Algorithm Based on Nonlinear Adaptive Square Root Unscented Kalman Filter

For fast simultaneous localization and mapping (FastSLAM) problem, to solve the problems of particle degradation, the error introduced by linearization and inconsistency of traditional algorithm, an improved algorithm is described in the paper. In order to improve the accuracy and reliability of algorithm which is applied in the system with lower measurement frequency, a new decomposition strategy is adopted for a posteriori estimation. In proposed decomposition strategy, the problem of solving a 3dimensional state vector and N 2-dimensional state vectors in traditional FastSLAM algorithm is transformed to the problem of solving N 5-dimensional state vectors. Furthermore, a nonlinear adaptive square root unscented Kalman filter (NASRUKF) is used to replace the particle filter and Kalman filter employed by traditional algorithm to reduce the model linearization error and avoid solving Jacobian matrices. Finally, the proposed algorithm is experimentally verified by vehicle in indoor environment. The results prove that the positioning accuracy of proposed FastSLAM algorithm is less than 1 cm and the azimuth angle error is 0.5 degrees.


Introduction
In simultaneous localization and mapping (SLAM), vehicle uses the carried sensors to sense surroundings and uses the sensed information to create environment map on one hand.On the other hand, vehicle uses created map to locate and guide.
There are several methods to deal with SLAM problem, in which EKF-SLAM and FastSLAM are the two most popular methods.EKF-SLAM has some obvious limitations: inconsistency due to errors accumulation introduced by linearization, complex computation to deal with high-dimensional joint covariance, lack of robustness to incorrect data association, and so on [1][2][3].FastSLAM employs the Rao-Blackwellized particle filter (RBPF) to estimate position and extended Kalman filter (EKF) to estimate map features.Compared with EKF-SLAM, FastSLAM has a lower complexity and is more robust regarding the data association problem.Nevertheless, the FastSLAM based on RBPF also suffers from some drawbacks such as particle degeneration, Jacobian Matrix solving and linear processing of nonlinear function [4][5][6].To deal with these problems, the SLAM based on square root unscented Kalman filter (SRUKF) is proposed.Instead of approximating the state and measurement transition functions by Taylor series expansion, the unscented transformation employed by SLAM is used to update nonlinear state and measurement functions and the accuracy of the state estimation has been significantly improved [7][8][9].
The filtering approaches used in all aforementioned SLAM methods can achieve good performance under certain assumptions.However, the assumptions are typically not entirely satisfied in practical applications.Thus, the performance of SLAM algorithm may be downgraded from the theoretical performance, which can potentially lead to divergence.To prevent divergence and to improve the practicability of SLAM algorithm, the so-called adaptive filtering approach has been used in SLAM algorithm to dynamically adjust the parameters of the supposedly optimum filter based on estimating the unknown parameters for online estimation of motion and by estimating the signal and noise statistics from the available data.The adaptive filter can be realized to adjust important coefficients through fuzzy logic algorithm or neural network algorithm [10,11].Another popular method to realize the adaptive filtering is to use estimators [12][13][14].
To overcome the shortcomings of traditional adaptive filter, we developed a new nonlinear adaptive square root unscented Kalman filter (NASRUKF) which can be used in nonlinear or linear system for multisensory data fusion with uncertain process noise [15,16].In [17], the prototype of FastSLAM algorithm based on NASRUKF is described, and the validity is demonstrated by simple preliminary experiment.
In this paper, the improved FastSLAM algorithm based on NASRUKF is described in detail.To verify the accuracy of FastSLAM under the condition of low measurement frequency, the algorithms based on SRUKF and ASRUKF are compared and analyzed.And a complete experiment is designed for further validation.

FastSLAM Framework
The idea of FastSLAM algorithm comes from an analysis result of Dynamic Bayesian Network (DBN): if the vehicle pose is fully determined, then the map features are mutually independent.As shown in Figure 1, if the vehicle pose estimation (x  (0), x (1), . . ., x ()) is known, the estimations of map features  1 and  2 are independent from each other.The input information of FastSLAM includes the control information u(0 :  − 1) and the measurement information z(1 : ).
From a probabilistic point of view, SLAM problem is to solve the posterior probability distribution of system state vectors composed of vehicle pose vector x  and map feature vector x  , which can be expressed as follows [18]: From the aforementioned DBA analysis result and Bayesian formula, formula (1) can be expressed as follows: (2) Formula (2) shows the idea of FastSLAM algorithm that the joint SLAM state estimation can be factored into two independent estimations: vehicle pose component and map feature component.Thus, the problem of solving (2 + 3) dimension state vectors is converted into two parts: a 3dimension state vector solving of vehicle pose and  2dimension state vectors solving of map features.
For traditional FastSLAM algorithm, estimation of vehicle pose is achieved by particle filtering.Each map feature, in each particle, can be estimated using Extended Kalman Filters conditioned on the robot pose of the particle.And the factor weight of particles is calculated to determine the probability that a certain particle enters the final set.
The implementation process of FastSLAM algorithm is as follows.
(1) The posterior probability distribution of vehicle pose is estimated using the control information and the motion model.(2) Map features of particles are associated with measurement information via maximum likelihood estimation.(3) According to correlated measurement information of each particle, update the estimations of each map feature and vehicle pose.

Optimization of FastSLAM
As the performance of FastSLAM depends on measurement information, the accuracy and reliability of traditional algorithm are downgraded when measurement data are limited by detecting frequency of sensors.In addition, the linearization method increases the estimation error for the reason that the actual system has strong nonlinearity and uncertain noise.
In [17], the basic idea of an modified FastSLAM was introduced simply.In this section, the modified FastSLAM introduced in [17] will be described in detail which is especially appropriate for low speed vehicle system (such as the wheelchair used for the aged).Considering low scanning speed sensors, a new decomposition strategy is designed for the posterior probability distribution.Moreover, an improved a posteriori estimation method is proposed which simultaneously estimates the joint state vectors comprised of vehicle pose and map features.

Posterior Probability Distribution Decomposition Strategy.
The proposed posterior probability distribution decomposition strategy of state vectors is shown as Assuming that there are  particle state estimations X1 , X2 , . . ., X and relevant covariance matrices of estimation error P 11 , P 22 , . . ., P  , the particle state estimations are irrelevant; that is, P  = 0 ( ̸ = ).Then optimal estimation of the joint state vectors can be expressed as The physical meaning of formula ( 4) is that if the estimation accuracy of X is low, its global contribution P −1  X will be low.
It can be drawn from ( 3) and ( 4) that As shown in ( 3) and ( 5), the proposed FastSLAM algorithm converts the high-dimensional joint state estimation into several independent low dimensional joint state estimations.That is, the state vectors solving of (2 + 3) dimensions is converted into  5-dimension state vectors solving.
Compared with the traditional FastSLAM algorithms, the correlation between vehicle pose and map features in low dimension state vectors is considered which is helpful to improve the accuracy and reliability of FastSLAM, especially when one frame data of the sensor can be used to observe at most one map feature which cannot create a new full map feature.This often appears in a vehicle system with lower measuring frequency sensors, such as sonar or infrared sensor, which can only obtain one map feature in a sampling cycle.
Equations ( 3) and ( 5) can be simplified as where Fo represents the created map feature; x Fo represents the th state vector with created feature.The meaning of formula ( 6) is that if the th measurement data is from created feature, then joint estimation of vehicle pose and the created feature is carried out.If not, the vehicle pose is estimated only and the th measurement data is saved to create a new map feature.

NASRUKF.
For a nonlinear and discrete system, stochastic sequence is described by where () and ℎ() are nonlinear functions of the system; () is the state vector of system; ( − 1) is the control vector applied at time  − 1; ( − 1) is process noise; () is observation vector; V() is the measurement noise.
The sigma points are calculated as The time update equations are the following: Augment sigma points are Estimate the square root of the measurement noise matrix: The measurement update equations are as follows: x () = x− () + K () z () , where  = √  + ; () = (1 − )/(1 −  +1 ) and  is the forgetting factor, typically 0 <  < 1; the weights ( ()  and  ()  ) of the mean and covariance are given by where  =  2 ( + ) −  is a scaling parameter.The constant  determines the spread of the sigma points around the mean, which is typically set to a small, positive value.The constant  ≥ 0 is a secondary scaling parameter. ≥ 0 is used to incorporate the prior knowledge of the distribution (for Gaussian distributions,  = 2 is optimal).

Implementation of New FastSLAM Algorithm
The proposed FastSLAM algorithm implementation process is shown as in Figure 2.
Initialization.During initialization, the initial pose of vehicle is obtained by the static state estimation method based on the measurement data of sensors.In addition, the initial feature of the map is created using the feature extraction method.After initialization, the vehicle location and mapping will be calculated and updated based on proposed FastSLAM algorithm in the following steps.
Vehicle Pose Estimation.The vehicle pose is estimated using ASRUKF.
Data Association.According to ( 14), (15), and ( 16), data association of the measurement data at time  is dealt with to determine whether it is a created map feature.If the measurement data is a created map feature, algorithm turns to vehicle pose and map feature updating; otherwise, the data shall be stored as new feature data, and algorithm turns to new map feature creating.
Vehicle Pose and Map Feature Updating.According to [17], this step consists of two cases.The first one is that the measurement data ((), ()) matches the created linear feature.The second case is that the measurement data matches the created arc feature.
(1) The Measurement Data Matches the Created Linear Feature.If the line segment is not parallel to   -axis, that is,  =    +   , it can be defined that x  () = [(), (), (),   (),   ()] T is the state variable of the nonlinear discrete system described as If the line segment is parallel to   -axis, that is,  =   , x  () = [(), (), (),   ()] T can be defined as the state variable of the nonlinear discrete system, which is described by where w  and V  have the statistical property of zero mean, mutual independence, and Gaussian distribution.The covariance matrix of w  and V  is Q  and   , respectively.
(2) The Measurement Data Matches the Created Arc Feature.
It can be defined that x  () = [(), (), (),   (),   (),   ()] T is the state variable of the nonlinear discrete system described as where New Map Feature Creating.If the number of new feature data exceeds threshold value  new , a new map feature should be created.

Experimental Results
In order to evaluate the performance of proposed FastSLAM algorithm, experimental results are shown in this section.The test environment is built as Figure 3.The main parameters are depicted as follows:  = 180 mm, the length threshold of linear segment is  min = 500 mm, the distance threshold between point and line is  max = 80 mm, and the threshold of average vector distance is  max = 30 mm.As shown in Figure 3, CPU Motion Controller implements the proposed FastSLAM algorithm and uploads the results to the computer via the receiving node  1 and the transmitting node  1 for real-time display.
The whole experiment process is divided into the static state estimation and the dynamic update.Then the scanned data set is segmented: (1) First, the point C is detected as the maximum distance from line AB which exceeds the threshold.Thus, the continuous data set AB is divided into two subsets of AC and CB.Then, the point D is detected as the maximum distance from line AC which exceeds the threshold and the data set AC is divided into two subsets of AD and DC.Similarly, the data set CB is divided into two subsets of CE and EB and the data set EB is divided into two subsets of EF and FB, as shown in Figure 4(a).At last, the distance between point C and line DE is detected to be less than the threshold.Thus, the subsets DC and CE are combined into one data set DE. (2) Second, the lengths of AD, DE, EF, and FB are calculated, which exceed the line threshold.Thus, the subsets AD, DE, EF, and FB are processed as line segment.Then the subsets FB are analyzed by most similar algorithm which belongs to the arc segment, as shown in Figure 4(b).

Dynamic Update
(1) Evaluation of Vehicle Pose Estimation Accuracy.The realtime pose is estimated using ASRKF and SRKF, respectively, when the vehicle travels along the desired trajectory.The statistic characteristic of process noise is unknown.The data measurement period is 25 ms.The result of experiment is shown as in Figure 5.It is observed from Figure 5 that the errors of vehicle state ( and ) estimated by NASRUKF are significantly smaller than the errors estimated by SRUKF.After 4 seconds, as the identification of process noise statistical feature, the conclusions are more obvious.(1) From Figure 5(a), the estimated error of - plane calculated by ASRUKF is within 1 cm, while the estimated error calculated by SRUKF is within 5 cm.(2) From Figure 5(b), the estimated error of azimuth calculated by ASRUKF is within 1.15 ∘ (0.02 rad), while the estimated error calculated by SRUKF is almost 5.16 ∘ (0.09 rad).
(2) Evaluation of Map Creating Accuracy.When vehicle slowly moves along the line, the vehicle track and environmental map feature are dynamically updated.The test results are shown in Figure 6 and Table 1.As shown in Figure 6 and Table 1, the localization accuracy based on the improved FastSLAM algorithm is within 1 cm and the azimuthal error is approximately 0.5 ∘ .The position error of the corner point in the map feature is approximately 3 cm.The errors in both the center position

5. 1 .
The Static State Estimation.The surroundings are scanned by the sensors of vehicle under stationary state.The scanned data set is clustered by adaptive breakpoint detector.

Table 1 :
Error in created environmental features.
lines (estimate map) denote the map feature estimated with the improved FastSLAM algorithm.Long dash lines (estimate track and estimate theta) denote the state estimation or estimation error using the improved FastSLAM algorithm.