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

Authors: Yu-feng Zhang; Qi-xun Zhou; Ju-zhong Zhang; Yi Jiang; Kai Wang
Journal: Mathematical Problems in Engineering
 (2017)

---

## Abstract

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 3-dimensional 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.


---

## Body

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


## 2. 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 Figure1, if the vehicle pose estimation (x^R0,x^R1,…,x^R(k)) is known, the estimations of map features F1 and F2 are independent from each other. The input information of FastSLAM includes the control information u(0:k-1) and the measurement information z(1:k).Figure 1
Schematic diagram of SLAM problem.From a probabilistic point of view, SLAM problem is to solve the posterior probability distribution of system state vectors composed of vehicle pose vectorxR and map feature vector xF, which can be expressed as follows [18]:(1)px1:k∣z1:k,u0:k-1,xR0.From the aforementioned DBA analysis result and Bayesian formula, formula (1) can be expressed as follows:(2)px1:k∣z1:k,u0:k-1,xR0=pxR1:k,xF1:k∣z1:k,u0:k-1,xR0=pxR1:k∣z1:k,u0:k-1,xR0·∏i=1NpxFi1:k∣xR1:k,z1:k.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 (2N+3) dimension state vectors is converted into two parts: a 3-dimension state vector solving of vehicle pose and N 2-dimension 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.


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


### 3.1. Posterior Probability Distribution Decomposition Strategy

The proposed posterior probability distribution decomposition strategy of state vectors is shown as(3)px1:k∣z1:k,u0:k-1,xR0=pxR1:k,xF1:k∣z1:k,u0:k-1,xR0=∏i=1NpxRik,xFi1:k∣z1:k,u0:k-1,xR0,k=1∏i=1NpxR1:k-1,xRik,xFi1:k∣z1:k,u0:k-1,xR0,k>1.Assuming that there areN particle state estimations X^1,X^2,…,X^n and relevant covariance matrices of estimation error P11,P22,…,Pnn, the particle state estimations are irrelevant; that is, Pij=0(i≠j). Then optimal estimation of the joint state vectors can be expressed as(4)X^g=∑i=1NPii-1X^i∑i=1NPii-1.The physical meaning of formula (4) is that if the estimation accuracy of  X^i is low, its global contribution Pii-1X^i will be low.It can be drawn from (3) and (4) that(5)x^Rk=∑i=1NPRi-1kx^Rik∑i=1NPRi-1k.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 (2N+3) dimensions is converted into N 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(6)px1:k∣z1:k,u0:k-1,xR0=pxR1:k,xFoi1:k∣z1:k,u0:k-1,xR0,zfromFopxR1:k∣z1:k,u0:k-1,xR0,othersi=1,2,…,N,where Fo represents the created map feature; xFoi represents the ith state vector with created feature. The meaning of formula (6) is that if the kth 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 kth measurement data is saved to create a new map feature.


### 3.2. NASRUKF

For a nonlinear and discrete system, stochastic sequence is described by(7)xk=fxk-1,uk-1+wk-1,zk=hxk+vk,where f(x) and h(x) are nonlinear functions of the system; x(k) is the state vector of system; u(k-1) is the control vector applied at time k-1; w(k-1) is process noise; z(k) is observation vector; v(k) is the measurement noise.The sigma points are calculated as(8)χk-1=x^k-1x^k-1+γSk-1x^k-1-γSk-1.The time update equations are the following:(9)χ^∗k=fχk-1,uk-1,x^-k=∑i=02LWimχ^i∗k,S-k=qrW1cχ^1:2L∗k-x^-kQ^k-1,S-k=cholupdateS-k,χ^0∗k-x^-k,W0c.Augment sigma points are(10)χ^-k=x^-kx^-k+γS-kx^-k-γS-k.Estimate the square root of the measurement noise matrix:(11)Z^-k=hχ^-k,z^-k=∑i=02LWimZ^i-k,z~k=zk-z^-k,R∗∗=cholupdate1-dkR^k-1,z~k,dk,R∗=cholupdateR∗∗,Z^0:2L-k-z^-k,-dkWic,R^k=diagdiagR∗R∗T.The measurement update equations are as follows:(12)Pxzk=∑i=02LWicχ^i-k-x^-kZ^i-k-z^-kT,Szk=qrW1cZ^1:2L-k-z^-kR^k,Szk=cholupdateSzk,Z^0-k-z^-k,W0c,Kk=Pxzk/SzkTSzk,x^k=x^-k+Kkz~k,U=KkSzk,Sk=cholupdateS-k,U,-1,where γ=L+λ; d(k)=(1-b)/(1-bk+1) and b is the forgetting factor, typically 0<b<1; the weights (Wi(m) and Wi(c)) of the mean and covariance are given by(13)W0m=λL+λ,W0c=λL+λ+1-α2+β,Wim=Wic=12L+λ,i=1,…,2L,where λ=α2(L+κ)-L 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).


## 3.1. Posterior Probability Distribution Decomposition Strategy

The proposed posterior probability distribution decomposition strategy of state vectors is shown as(3)px1:k∣z1:k,u0:k-1,xR0=pxR1:k,xF1:k∣z1:k,u0:k-1,xR0=∏i=1NpxRik,xFi1:k∣z1:k,u0:k-1,xR0,k=1∏i=1NpxR1:k-1,xRik,xFi1:k∣z1:k,u0:k-1,xR0,k>1.Assuming that there areN particle state estimations X^1,X^2,…,X^n and relevant covariance matrices of estimation error P11,P22,…,Pnn, the particle state estimations are irrelevant; that is, Pij=0(i≠j). Then optimal estimation of the joint state vectors can be expressed as(4)X^g=∑i=1NPii-1X^i∑i=1NPii-1.The physical meaning of formula (4) is that if the estimation accuracy of  X^i is low, its global contribution Pii-1X^i will be low.It can be drawn from (3) and (4) that(5)x^Rk=∑i=1NPRi-1kx^Rik∑i=1NPRi-1k.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 (2N+3) dimensions is converted into N 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(6)px1:k∣z1:k,u0:k-1,xR0=pxR1:k,xFoi1:k∣z1:k,u0:k-1,xR0,zfromFopxR1:k∣z1:k,u0:k-1,xR0,othersi=1,2,…,N,where Fo represents the created map feature; xFoi represents the ith state vector with created feature. The meaning of formula (6) is that if the kth 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 kth measurement data is saved to create a new map feature.


## 3.2. NASRUKF

For a nonlinear and discrete system, stochastic sequence is described by(7)xk=fxk-1,uk-1+wk-1,zk=hxk+vk,where f(x) and h(x) are nonlinear functions of the system; x(k) is the state vector of system; u(k-1) is the control vector applied at time k-1; w(k-1) is process noise; z(k) is observation vector; v(k) is the measurement noise.The sigma points are calculated as(8)χk-1=x^k-1x^k-1+γSk-1x^k-1-γSk-1.The time update equations are the following:(9)χ^∗k=fχk-1,uk-1,x^-k=∑i=02LWimχ^i∗k,S-k=qrW1cχ^1:2L∗k-x^-kQ^k-1,S-k=cholupdateS-k,χ^0∗k-x^-k,W0c.Augment sigma points are(10)χ^-k=x^-kx^-k+γS-kx^-k-γS-k.Estimate the square root of the measurement noise matrix:(11)Z^-k=hχ^-k,z^-k=∑i=02LWimZ^i-k,z~k=zk-z^-k,R∗∗=cholupdate1-dkR^k-1,z~k,dk,R∗=cholupdateR∗∗,Z^0:2L-k-z^-k,-dkWic,R^k=diagdiagR∗R∗T.The measurement update equations are as follows:(12)Pxzk=∑i=02LWicχ^i-k-x^-kZ^i-k-z^-kT,Szk=qrW1cZ^1:2L-k-z^-kR^k,Szk=cholupdateSzk,Z^0-k-z^-k,W0c,Kk=Pxzk/SzkTSzk,x^k=x^-k+Kkz~k,U=KkSzk,Sk=cholupdateS-k,U,-1,where γ=L+λ; d(k)=(1-b)/(1-bk+1) and b is the forgetting factor, typically 0<b<1; the weights (Wi(m) and Wi(c)) of the mean and covariance are given by(13)W0m=λL+λ,W0c=λL+λ+1-α2+β,Wim=Wic=12L+λ,i=1,…,2L,where λ=α2(L+κ)-L 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).


## 4. Implementation of New FastSLAM Algorithm

The proposed FastSLAM algorithm implementation process is shown as in Figure2.Figure 2
Implementation process of FastSLAM algorithm.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 k 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 (ρ(k),φ(k)) 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 YI-axis, that is, y=ajx+bj, it can be defined that xj(k)=x(k),y(k),θ(k),aj(k),bj(k)T is the state variable of the nonlinear discrete system described as (14)xjk=xjk-1+wjk-1,zjk=ρk=dScos⁡θj=dScos⁡αj-θk-φk=ajk·xk-Ls·cos⁡θk-yk-Ls·sin⁡θk+bjk-ajkcos⁡θk+φk+sin⁡θk+φk+vjk.If the line segment is parallel toYI-axis, that is, x=cj, xj(k)=x(k),y(k),θ(k),cj(k)T can be defined as the state variable of the nonlinear discrete system, which is described by (15)xjk=xjk-1+wjk-1,zjk=ρk=xk-Ls·cos⁡θk-cjkcos⁡θk+φk+vjk,where wj and vj have the statistical property of zero mean, mutual independence, and Gaussian distribution. The covariance matrix of wj and vj is Qj and Rj, respectively.(2) The Measurement Data Matches the Created Arc Feature. It can be defined that xcj(k)=x(k),y(k),θ(k),xcj(k),ycj(k),Rcj(k)T is the state variable of the nonlinear discrete system described as (16)xcjk=xcjk-1+wcjk-1,zcjk=ρk=dScos⁡θj±dS2cos2⁡θj-dS2-Rcj2k+vcjk,where(17)dS=xk-Lscos⁡θk-xcjk2+yk-Lssin⁡θk-ycjk2.New Map Feature Creating. If the number of new feature data exceeds threshold value Nnew, a new map feature should be created.


## 5. 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 Figure3. The main parameters are depicted as follows: Ls=180mm, the length threshold of linear segment is Lmin=500mm, the distance threshold between point and line is dmax=80mm, and the threshold of average vector distance is mdmax=30mm.Figure 3
Schematic diagram of test data transmission.As shown in Figure3, CPU Motion Controller implements the proposed FastSLAM algorithm and uploads the results to the computer via the receiving node R1 and the transmitting node T1 for real-time display.The whole experiment process is divided into the static state estimation and the dynamic update.


### 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.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 Figure4(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 Figure4(b).Figure 4
The static state estimation of map features.
(a)
(b)


### 5.2. Dynamic Update

(1) Evaluation of Vehicle Pose Estimation Accuracy. The real-time 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 Figure5. It is observed from Figure 5 that the errors of vehicle state (xy 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 x-y 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).Figure 5
Results of vehicle pose estimation.
(a)
Results ofxy estimation
(b)
Results ofθ estimation(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.Table 1
Error in created environmental features.
(a)
Feature
LineLAD
LineLDE
LineLEF
Arc FB
Slope
Intercept
Slope
Intercept
Slope
Intercept
Center
Radius
Error
0.0616
−10.6518 mm
−0.0292
7.8651 mm
0.0247
−15.5775 mm
15.5822 mm
−13.3833 mm
(b)
Feature
Break point A
Corner point D
Corner point E
Corner point F
Error
21.7072 mm
20.3451 mm
18.5317 mm
31.0601 mmFigure 6
Results of SLAM dynamic update.
(a)
Results of map creating
(b)
Results ofxy estimation
(c)
Results ofθ estimationNote. Full lines in the figure (real map, real track, and real theta) denote the true value of actual state. Short dash 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.As shown in Figure6 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 and radius of the arc are within 2 cm. The slope error of line segment is within 0.07. The intercept error is within 2 cm. It is demonstrated by the test results that the proposed FastSLAM algorithm is effective.


## 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.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 Figure4(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 Figure4(b).Figure 4
The static state estimation of map features.
(a)
(b)


## 5.2. Dynamic Update

(1) Evaluation of Vehicle Pose Estimation Accuracy. The real-time 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 Figure5. It is observed from Figure 5 that the errors of vehicle state (xy 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 x-y 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).Figure 5
Results of vehicle pose estimation.
(a)
Results ofxy estimation
(b)
Results ofθ estimation(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.Table 1
Error in created environmental features.
(a)
Feature
LineLAD
LineLDE
LineLEF
Arc FB
Slope
Intercept
Slope
Intercept
Slope
Intercept
Center
Radius
Error
0.0616
−10.6518 mm
−0.0292
7.8651 mm
0.0247
−15.5775 mm
15.5822 mm
−13.3833 mm
(b)
Feature
Break point A
Corner point D
Corner point E
Corner point F
Error
21.7072 mm
20.3451 mm
18.5317 mm
31.0601 mmFigure 6
Results of SLAM dynamic update.
(a)
Results of map creating
(b)
Results ofxy estimation
(c)
Results ofθ estimationNote. Full lines in the figure (real map, real track, and real theta) denote the true value of actual state. Short dash 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.As shown in Figure6 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 and radius of the arc are within 2 cm. The slope error of line segment is within 0.07. The intercept error is within 2 cm. It is demonstrated by the test results that the proposed FastSLAM algorithm is effective.


## 6. Conclusions

An improved FastSLAM algorithm based on NASRUKF is proposed in paper. To improve the accuracy and reliability of the FastSLAM limited by measuring frequency, a new posterior probability distribution decomposition strategy is proposed. And the vehicle pose and map features are estimated using NASRUKF algorithm, which overcomes the limitations of the traditional FastSLAM algorithm (e.g., solutions to Jacobian matrix are needed; failure to meet consistency conditions and occurrence of “degeneration of particle set”). The adaptive ability of filters is improved while SLAM accuracy is maintained, which provides an effective way to solve SLAM problem. The effectiveness of the proposed FastSLAM algorithm is verified by indoor experiment results.



---

*Source: 4197635-2017-03-15.xml*