The Square Root UKF-SLAM Algorithm Based on the Smallest Proportion of Skewness in Single Line Sampling

In view of the distortion in the filter gain matrix calculation as well as the high computational complexity and the nonlocal effect of symmetric sampling that exists in the UKF-SLAM algorithm,the square root UKF-SLAM algorithm based on the smallest proportion of skewness in single line sampling was proposed. According to the mended algorithm, the square root of covariance matrix is brought into iteration operation instead of covariance matrix, moreover, the smallest proportion of skewness in single line sampling is utilized for the optimization of sampling strategy. The results of simulation show that the algorithm can effectively improve the position accuracy in robot as well as the estimation accuracy of feature map. Furthermore, the computational complexity is reduced and the algorithm stability is improved.


Introduction
In the unknown environment, the robot takes advantage of its own sensors to create feature maps for the environment incrementally. At the same time, the machine corrects its own position that has been created in the map, namely SLAM, a problem about simultaneous localization and mapping in mobile robot. Moreover, a classical algorithm that had been applied to SLAM is the method that dynamic equations and observation equations of robot are expanded by first order Taylor series, and then the Extended Kalman Filter algorithm was used for the optimal estimation of the robot's position and the feature map. This model was first proposed by Smith and Cheeseman [1]. Many scholars optimized and improved it based on EKF-SLAM framework. For example, Lu presented that polar coordinates can be used to compare two adjacent observed values to reduce external interference and improve the algorithm's estimation accuracy as well as robustness [2]; Zhanghai Jiang put forward an improved compression-type EKF-SLAM algorithm to reduce the computational complexity [3]. Zhou Wu proposed a global observation map model to improve the presentation methods [4] of the map. However, the inherent defects, such as the truncation error generated from the linearization procedure and the large amount of calculation from Jacobi matrix, make the EKF-SLAM algorithm unsuited to the large-scale environmental map.
Therefore, Julier et al. pointed out Unscented Kalman Filter(UKF), a new filtering algorithm that can be directly applied to nonlinear systems when they studied how to improve EKF-SLAM. UKF is a nonlinear Gaussian state estimator based on the minimum variance estimation criteria [5]. Similar to the Extended Kalman Filter algorithm, Gaussian random variables are used to approximate the state distribution. But the difference is that UKF approximate the state distribution through deterministic sampled (Sigma points) obtained from some certainty sampling. These sampling points can better describe the true mean and variance of the Gauss random variable. In addition, its precision approximates to the second-order of Taylor while the EKF can only approximates to the first-order accuracy under the same conditions. Besides ， in terms of computational complexity, UKF and EKF belong to the same order and do not need to calculate the Jacobian matrix, so UKF is easier to meet our objectives than the EKF [6]. Furthermore, the follow-up work found that the state covariance matrix of UKF algorithm is easy to lose non-negative qualitative and symmetry due to the rounding errors from the computer in the process of data transmission, which will lead to filter divergence [7]. On the contrary, the square root filter and the strategy that use the square root of the covariance matrix instead of covariance matrix itself can solve this problem perfectly, which has been successfully applied to GPS / DR combination [8] and indoor geolocation [9].
In the first place, this article focuses on MATLAB comparative simulation experiments that simulate the process of SLAM which UKF algorithm and the square root UKF algorithm (SR-UKF) apply to. The result shows that the SR-UKF algorithm can significantly improve the accuracy of the robot's pose estimation. However, in terms of SLAM, a kind of system which need high requirements of real-time, it is particularly important to reduce the computational complexity and improve the computational efficiency [10]. To solve the above problems, the SR-UKF algorithm was given a depth study by changing the sampling strategy and using the smallest proportion of skewness in single line sampling instead of symmetric sampling, as well as reducing the computational complexity through reducing the number of points of Sigma, which enables the systems towards the direction of better real-time. Therefore, a square root UKF-SLAM algorithm based on the smallest proportion of skewness in single line sampling has been proposed in this paper.

The probability description about SLAM
It assumes that the state vector of a robot is 1:k Z and 1:k u are known conditions. We calculate posterior probability distribution in formula (1) by Baye filter principle to obtain the optimal state estimation. The whole process is divided into two steps, the first step is prediction, the prior probability distribution at k time is obtained by computational model of robot pose and posterior probability distribution at k+1 time: The second step is observation update, k Z is gained from the observation model of system at k time, and the posterior probability distribution is solved by the prior probability distribution.
 is the normalization coefficient.

UKF-SLAM algorithm
UT Transformation as the core, kalman filter as the basic framework, UKF take advantage of certainty sampling to get the posterior state distribution from state estimation. Before filtering, the original state vector is augmented to gauss noise variable: It assumes that the mean is 1 k a X   and the variance is 1 a k P  at k-1 time. The UKF-SLAM algorithm block diagram was shown in Figure 1. The whole algorithm is divided into three parts , including sigma point、forecast period and update phase calculations. Besides, the calculation of sigma point adopts the symmetrical sampling method. These sampling points are taken into the model of nonlinear system for transition, which helps to get the statistical properties of the state vector.

SR-UKF-SLAM algorithm
In the aforesaid filtering model, the rounding errors of calculation are gradually accumulated with the depth of filtering. In addition, the possibility that the filtering error covariance matrix k P and the predicting error covariance matrix | 1 k k P  lose non-negative qualitative and symmetry will increase, which makes the calculation of filtering gain distorted as well as the filter diverging. Therefore, on the basic of UKF-SLAM algorithm, with the update of covariance square root k S instead of covariance , a UKF-SLAM algorithm based on the square root [11] (SR-UKF-SLAM) is proposed. Furthermore, in the specific process of algorithm, the two calculation methods including QR decomposition and Cholesky factor update are utilized to reduce the computational complexity. As distinguished from the EKF-SLAM algorithm, its optimization process is as follows: Predict phase: Update phase: In order to avoid the matrix inverse operation, back substitution was used to deal with filtering gain k K . As shown in the formula (10), formula (11)、formula (13) are applied to the iterative update of state vector.

Simulation and Analysis
In order to study the effect of the estimation accuracy from square root transition, the SR-UKF-SLAM and UKF-SLAM algorithm were respectively simulated through open source simulation platform developed by Tim Bailey in MATLAB. The computer in the experiment employs Intel Core-i5 dual-core processor with 2.6 GHz basic frequency and 4 GB internal storage.
In the experiment, with 250*200m analog outside circumstance, the robot paraded a circle along 17 path points ("  ") from coordinates (0,0), the running speed of the robot is up to 2 m/s, 35 static feature points("*") were randomly distributed in the environment. The simulation results of two algorithms are shown in Figures 2 and 3, the filament represents the ideal trajectory of the robot, the "dotted line" indicates the estimated path of the robot, "+" is the position of feature point, "ellipse" is the error range of the feature point estimation. Figure 2 and figure 3 shows that, during the initial period, the calculation error between UKF-SLAM and SR-UKF-SLAM algorithm became relatively small. However, as time goes on, due to the constantly increased new features, there has been an increasingly deviation gap between estimated and ideal trajectory. and the error ellipse from the features map is also significantly higher than the SR-UKF-SLAM algorithm.

Simulation experiment and analysis of SPSR-UKF-SLAM algorithm
Although the SR-UKF-SLAM algorithm improves the estimation accuracy of robot-position and features maps to a certain degree, however, in terms of SLAM which requires hard real-time performance, we hope that the computational complexity can be further reduced through reducing the number of sigma points. In the symmetric sampling, the distance from sigma point to k a X  ascends with the increasement of state dimension of k a X , which easily causes nonlocal effect and affects the accuracy and stability of the filtering. The smallest proportion of skewness in single line is a kind of sampling mode that needs minimal points，due to the direct influence on the calculation amount of UKF from the number of sampling points, the strategy is more suitable for the system with higher real-time performance.
According to Julier's analysis in the literature [11], at least n+1 sampling points are required to determine the posterior distribution of the system for the n-dimensional state vector distribution. Moreover, the study found that the proportion correction can be a good solution to the non-local effects. Therefore, combined with the advantages of the sampling strategy above, a square root UKF-SLAM algorithm based on the smallest proportion of skewness in single line sampling, namely the SPSR-UKF-SLAM algorithm, was proposed in the process of SLAM，Which helps modify the sampling strategy of SR-UKF-SLAM algorithm, its specific sampling process is as follows: (1) The weight coefficient of initialization (2) The first and second order weight coefficient of sigma sampling points are: According to the state dimension of input 2,3, , L n   , the iteration formula of vector is: The simulation experiments was carried out on the optimized algorithm in the same simulation environment, the simulation results shown in Figure 5. With respect to the results shown in Figure 2, the estimated trajectory from robot goes a step further approaching the ideal MATEC Web of Conferences trajectory in the process of SPSR-UKF-SLAM algorithm，and the error ellipse is further reduced. (a) , (b) , (c)in figure 6 respectively indicate the calculation accuracy of robot in x and y direction as well as the error comparison of course angle that were obtained from SR-UKF-SLAM (as SR-UKF showed in the picture, dotted line) and SPSR-UKF-SLAM (as SPSR-UKF-SLAM showed in the picture, solid line) algorithm. From the picture, SPSR-UKF-SLAM improves the estimation precision of the robot pose state while reducing the computational complexity.  In order to compare the performance of the three SLAM algorithms, the average errors of the three algorithms are calculated as shown in table 1. From the data in table 1, The estimation accuracy of robot position from SPSR-UKF-SLAM is the highest in three algorithms. Compared with SR-UKF-SLAM algorithm, the errors of robot-position in X direction, Y direction and the course angle are respectively reduced 19.08%, 29.31% and 50.47% under SPSR-UKF-SLAM algorithm.

Conclusion
According to the filter divergence from the rounding errors of caculation that exists in EKF-SLAM algorithm, the covariance square root was utilized for update instead of covariance. Furthermore, in term of the problems such as a great quantity of sampling site, the poor real-time performance and the nonlocal effect that easily produced, the sampling schematism was transformed into the one based on smallest proportion of skewness in single line sampling, from which a more optimized algorithm ,the square root UKF-SLAM algorithm based on the smallest proportion of skewness in single line sampling, was proposed. Moreover, the MATLAB simulation experiments show that, compared with the UKF-SLAM algorithm, the new algorithm can effectively improve the estimation accuracy of the robot's position and the feature maps with its relatively lower computational complexity and greater stability.