AN APPROACH ON ADVANCED UNSCENTED KALMAN FILTER FROM MOBILE ROBOT-SLAM

In the past 30 years, Kalman filter is a classical method to solve the problem of simultaneous localization and mapping (SLAM) of mobile robots. Extended Kalman filter (EKF) and unscented Kalman filter (UKF) are derived from Kalman filter. Extended Kalman filter (EKF) overcomes the nonlinear problem that Kalman filter cannot solve. However, when it is strongly nonlinear, EKF violates the assumption of local linearity, and EKF algorithm may make the filter diverge. Secondly, because the Jacobian matrix is needed in the online processing of EKF, its tedious calculation process makes the implementation of this method relatively difficult. Unscented Kalman filter (UKF) uses nonlinear model directly, avoids operation of Jacobian matrix of complex nonlinear function, and ensures the general adaptability of nonlinear system. In this paper, based on the square root unscented Kalman filter, sigma points are selected according to the square-root decomposition of prior covariance, and then weighted mean and covariance are calculated. The quaternion is used to represent the attitude, and the quaternion vector is converted to the rotation space for matrix operation. Comparing the robot poses estimated based on the square root traceless Kalman filter (QSR-UKF), square root traceless Kalman filter (SR-UKF), and extended Kalman filter (EKF), the simulation results show the QSR-UKF proposed in this paper is effective.


INTRODUCTION
Mobile robots can be divided into three types: remote control, semi-autonomous and autonomous. At present, the hotspot of mobile robot research is focusing on autonomously tasks solution assigned to it by human beings. The difficulty of autonomous mobile robot is how to make the robot autonomously fulfil different needs in a complex environment. Autonomous mobile robot refers to a robot that has a high degree of self-planning, self-organization, and self-adaptability, and is suitable for working in a complex unstructured environment. It is a comprehensive system that combines perception, thinking, and decision-making [Wang, 2003]. In the late 1960s, Nilsson and others developed the first autonomous mobile robot named Shakey at Stanford College for the purpose of studying autonomous reasoning, planning, and control of artificial intelligence technology in complex environments [Nilsson, 1996]. The Shakey robot has a perception system such as an electronic camera and a rangefinder. It applies artificial intelligence technology to the robot system so that it can perform independent reasoning, motion planning and real-time control in an unstructured environment. Shakey robots have simple perception, thinking and decision-making capabilities, for example, they can discover and grab wooden blocks according to human instructions. In the 21st century, with the rapid development of microelectronics technology, artificial intelligence technology, new materials technology, and computer technology, more researches focus their eyes on mobile robots. The French LAAS laboratory has launched the HILARE mobile robot [Giralt, Sobek, 1979]. HILARE is equipped with a TV camera and laser rangefinder and other sensing systems, which can realize the path planning and navigation functions of the global road marking system. In the 1980s, the development of mobile robots extended from the beginning of industrial robots years, the construction and positioning of robot maps have been widely discussed and made breakthrough progress. Both Durrant-Whyte [Durrant-Whyte, 1987] and Smith, Cheesman [Smith, Cheesman, 1987] proposed a consistent and probabilistic map construction method and introduced the theory of probability estimation. In 1995, the term "Simultaneous Localization and Mapping" was first used by Durrant-Whyte et al. [Durrant-Whyte, Rye, 1996] in a review of mobile robot research, and gave the basic framework of the SLAM problem and the results of convergence verification.
The research of mobile robot SLAM mainly includes data association [Kaess, Dellaert, 2009], environmental feature extraction and representation [Movafaghpour, Masehian, 2012], target detection [Bakar, Saad, 2012], closed loop [Williams, Cummins, 2009] and so on. There are many uncertain factors in the SLAM problem of mobile robots, including the uncertainty of the environment, the influence of sensor noise, and the uncertainty of the motion model and the observation model. Generally, the probability method is used to describe the uncertainty. The probability-based SLAM algorithm uses Bayesian filtering theory as the basic principle. The main algorithms include: SLAM algorithm based on Extended Kalman filter (EKF), SLAM algorithm based on Particle filter (PF), SLAM algorithm based on Extended Information filtering (EIF), and SLAM algorithm based on Expectation Maximization (EM).
Kalman filtering (KF) is a data processing algorithm in mobile robots SLAM. The algorithm expresses the pose of mobile robots and the feature positions in the map with state vectors. It can estimate the posterior probabilities of all elements in real time, the estimation and correction can be performed cyclically. Kalman filter assumes that the system is a linear system, and the state and observation noise conform to the Gaussian distribution. However, in practical applications, the error is relatively large, so Extended Kalman filter (EKF) came into being. Extended Kalman filter algorithm was first proposed by Smith [Smith, Cheeseman, 1990], which mainly approximates the nonlinear system by Taylor expansion of the nonlinear system. Its algorithmic process can be summarized as "forecast-update". Estimating the pose of the robot at the next moment according to the motion model, and calculating the predicted estimate of the entire state variable and its variance matrix, and then obtain the predicted value of the feature position according to the observation model, calculate the difference between the actual observation and the predicted observation. Integrated covariance of the system and calculates the Kalman filter enhancement, then uses it to correct the predicted estimates of the state variables. The mobile robot continuously circulates the process above, so as to eliminate accumulated errors as much as possible and get the most accurate positioning and composition results. In order to reduce the number of calculations of EKF SLAM algorithm, Jose.E et al. proposed a simple SLAM algorithm based on compressed EKF (CEKF) [Jose, Eduardo, 2001]. Paz.L.M et al. proposed a Divide and Conquer EKF-SLAM [Paz, Tardos, Jose, 2008]. In order to reduce the impact of noise on the accuracy of EKF, Yingming proposed to treat the noise of the robot system as true noise and use the method of increasing the state variable dimension to convert the noise model into a Gaussian white noise model. The results show that it can improve the positioning accuracy [Yingming, Ding, 2010].
Because Kalman filter has the disadvantages of large linearization error and large amount of calculation, J.K. Uhlmann and S.J. Julier proposed an Unscented Kalman filter (UKF) [Julier, Uhlmann, 1997] that performs better in nonlinear systems. Unscented Transformation (UT) is the basic idea, directly using the system's nonlinear model, without the need to calculate a complex Jacobian or Hessians matrix, through a deterministic sampling strategy, select a set of Sigma points that can represent the statistical characteristics of the state variables and substitute non-linear function, and using the set of Sigma points to approximate the posterior probability density of the target state. Compared with EKF, the UKF filtering method can get a better estimate and less calculation. But there are two shortcomings of UKF SLAM. First, its calculation amount is the cubic of the state vector. As the observation road markings increase, the calculation amount will increase as well. Second, the UKF has an inconsistency in estimating the state. As a result, the expanded form of the Sigma point filtering method has also been widely used. Li [Li, Ni, 2010] proposed the square root of Sigma point Kalman filtering. Square root filter can ensure the non-negative qualitative and symmetry of the covariance matrix, increase its stability and estimation accuracy, and it has applied in the system state and parameter estimation, target tracking and other fields.
Besides the SLAM method based on Kalman filtering, Particle filters (PF) are widely used in SLAM system as well. Particle filtering is a Sequential Lmportance Sampling (SIS) using Bayesian sampling estimation method [Murphy, 2000]. Montemerl et al. [Montemerl, Thrun, Koller, 2002] proposed a new method called FastSLAM algorithm, it divided the SLAM problem into two parts: robot pose estimation problem and positioning-based landmark position estimation problem. The algorithm uses the motion trajectory of each particle to represent the robot's motion path, uses EKF to estimate and update, and uses the observation information to calculate the weight of each particle to evaluate the quality of each path. However, the particle filtering uses a sequential importance sampling method, there must be "lack of particle diversity" and "depletion of particles". Also, if the number of particles is increased in order to avoid the above problems, the calculation time will increase a lot.
This paper aims to solving the nonlinear optimization problem in the process of mobile robot motion. The SLAM algorithm of Extended Kalman filtering and Unscented Kalman filtering are introduced. Unscented Kalman filtering involves the square root operation of higher-order matrices, which increases the computational burden. In this paper, a square root filter is added on the basis of unscented Kalman filtering. At the same time, quaternions have the advantages of low computational complexity, high precision, non-singularity and full-attitude operation. In this paper, the square-root of the covariance matrix is calculated by Cholesky decomposition, and a quaternion Square-root Unscented Kalman filtering (QSR-UKF) algorithm with better numerical stability is proposed. After the comparison of EKF, UKF and QSR-UKF by simulation experiments, results show that QSR-UKF algorithm has a better precision.

Linearization modelling of mobile robot SLAM
The problem of mobile robots SLAM is to start moving from an unknown starting point in an unknown environment. During the process of movement, the surrounding information is sensed according to the observation of the sensor, and a map is formed in an incremental manner, perception and positioning are performed simultaneously. For the SLAM problem, define the following state vector at time k: : The control vector of the robot from time k-1 to time k, : Vector describing the feature position in time i, and the true position of the feature does not change with time, : Observation vector of the current position of the robot to the feature.
Then the SLAM problem can be described as the formula (1).
According to the Bayes theorem and Markov assumption, the calculation of the estimated posterior probability density distribution P (xk| zk, uk) can be solved by two stages of prediction and update: Prediction stage: Based on the robot's motion model and current position, the state of the robot at the next moment is predicted.
The prior probability density of the robot system is: Update stage: According to the observation model of the robot, the information obtained by the sensor is used to update the state estimation of the system. The posterior probability density of the system is: P( | ) is the observation model. According to Markov assumption, when current state is known, the previous observation and the current observation are independent of each other, so P( | −1 , ) is the normalization constant.
If motion model and observation model satisfy the linear distribution, the Kalman filter algorithm can be used for solving. When it is not satisfied, the integral calculation is difficult to achieve, and the extended Kalman filter can be used for solving.

Extended Kalman filtering
Extended Kalman filtering is a nonlinear filtering method. Its principle is to perform Taylor expansion of the system state equation and measurement equation at the prediction point ̂, −1 and ignore all nonlinear expansion high-order terms to achieve linearization approximation of nonlinear equations. EKF needs to calculate the Jacobian matrix for derivation, and use Kalman recursion formula for filter recursion calculation [Armesto, Tomero, 2004].
The model of the nonlinear discrete system is: Among them, and are subject to Gaussian white noise respectively.
Taylor's expansion of the state transfer function f( −1 ) at ̂− 1 Letϕ k|k−1 = ∂f ∂x | x=x k−1 , then the state equation (4) can be written as: Ignoring the influence of higher order terms, equation (7) can be abbreviated as Using state transition function, the one-step prediction value of the state is: Therefore, the state vector one-step prediction error variance matrix can be expressed as： Similarly, expand the measurement function h( ) at ̂− 1 into Taylor expansion： The one-step prediction value of the measurement vector obtained from the measurement function is: Therefore, prediction error variance matrix of the measurement vector is: State enhancement matrix is: Therefore, estimated state vector at time k is: The state error covariance matrix is updated to: Summarizing the above reasoning process, the basic flow of the EKF filtering algorithm is： 1. Initialization: Set the state initial value x_0 and error variance matrix P_0 2.
Time update： The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLIII-B4-2020, 2020 XXIV ISPRS Congress (2020 edition) 3. Measurement update: 4. States Enhancement: EKF is the application of Kalman filtering in nonlinear systems. The algorithm only achieves the first-order estimation accuracy for nonlinear systems, ignoring the influence of Taylor higherorder expansion terms. When encountering a strong nonlinear system, the neglected higher-order truncation error will have large impact on the filter accuracy, and even cause the filter to not converge. Moreover, the calculation of the Jacobian matrix is a difficult problem and extremely error-prone. In the calculation process of Jacobian matrix, modern number operations are frequently generated, which is easy to produce errors. On this basis, unscented Kalman filtering is proposed.

Unscented Kalman filter
Considering that Extended Kalman filtering is prone to produce larger linearization errors in stronger nonlinear systems, and more cumbersome Jacobian matrix must be calculated, in recent years, an Unscented Kalman filtering (UKF) algorithm based on UT transformation has been proposed. That algorithm first selects a set of deterministic sampling points based on prior mean and variance of the system state, called Sigma points. To make sure statistical characteristics of this group of sampling points are consistent with the prior statistical characteristics of state. Linear transfer function obtains a new set of sampling points, and finally uses weighted statistical linear regression (WSLR) technique to obtain the statistical characteristics of this set of sample points as the posterior statistical distribution of the nonlinear function. Unscented Kalman filtering (UKF) approximates the probability density distribution of non-linear functions instead of approximating non-linear functions. There is no need to know explicit expression of non-linear functions, and it is possible to deal with non-differentiable nonlinear functions without calculating Jacobian matrix.
The number of particle points (generally called Sigma points) sampled by UKF is very small, and the specific number depends on the sampling strategy selected. The most commonly used is 2n + 1 Sigma point symmetric sampling. The calculation amount of UKF is basically equivalent to that of Extended Kalman filtering algorithm, but its performance is better than EKF, and it uses deterministic sampling to avoid the problem of particle point degradation of Particle filtering [PF].
The basic idea of UT transformation is: select a set of point sets (Sigma point sets) on the premise of ensuring the sampling mean x and covariance P, and perform nonlinear transformation on these Sigma point sets to obtain the mean and covariance of the transformed points, applied to each Sigma point of sampling, and then obtain a set of points after nonlinear conversion. This deterministic sampling method extracts state-specific statistical characteristic information, and can obtain more observation hypotheses than the EKF algorithm. Therefore, the estimation of the state statistical characteristics is more accurate than the EKF algorithm.
The selection and scale of Sigma points can be selected according to the following formula： λ=α 2 (n+χ)-n Sigma point sequence is： The weights of covariance and the mean of the Sigma point sequence are calculated as: Among them, α is scale parameter, it determines the spread of Sigma points, n is dimension of state vector, β≥0, x obey Gaussian distribution, β is optimal when 2 is taken. The square root of matrix can be obtained by Cholesky decomposition.
Based on the non-discrete linear system, applying the UT transform to the nonlinear system, the unscented Kalman filter algorithm can be obtained： 1. Initialization: 2. Time update: Selection of Sigma point and weight in time update: Calculating one-step prediction of the system state at time k, mean and covariance by non-linear measurement function χ | −1 ( )=f(χ −1 ( )), The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLIII-B4-2020, 2020 XXIV ISPRS Congress (2020 edition) 3. Measurement update: Selection of Sigma point and weight in measurement update: (37) Calculating one-step prediction of the system state at time k, mean and variance and cross-covariance by non-linear measurement function Z | −1 (i)=h(ξ | −1 (i))： Compared with EKF, the estimation accuracy of UKF reaches the second order or above, and have a better filtering performance from the formula above. In addition, the UKF algorithm also avoids the calculation problem of the EKF algorithm value nonlinear function Jacobin matrix, and expands the application range of the UKF algorithm in some extent.

SQUARE-ROOT-UKF
Sigma point sampling involves the square root operation of the error variance matrix P, and the square root operation of the higher order matrix will undoubtedly greatly increase the calculation burden of the system and affect the real-time performance of the system. AT the same time, numerical rounding error during calculation may also destroy the nonnegative qualitative and symmetry of the error covariance matrix, and reducing the stability of the filter. Therefore, the UKF algorithm based on square-root was proposed by R. Merwer and E. Wan [Merwer, Wan, 2001]. This algorithm uses singular value decomposition and Cholesky decomposition to implement the square root operation of the error variance matrix, so that the numerical operation characteristics of the UKF algorithm can be significantly improved.

Initialization
Where chol {} represents the Cholesky decomposition of the matrix.

Time update:
Selection of Sigma point and weight in time update: Calculating one-step prediction of the system state at time k, mean and cross-covariance by non-linear measurement function Among them, qr {…} represents the singular value decomposition of the matrix, cholupdate {…} represents the update of the matrix.

Measurement update:
Selection of Sigma point and weight in measurement update: Calculating one-step prediction of the system state at time k, mean and cross-covariance by non-linear measurement function Z | −1 (i)=h(ξ | −1 (i)),

QUATERNION SQUARE-ROOT-UKF
To solve pose estimation of mobile robots, traditional nonlinear optimization method usually faces the problem of large amount of calculation. Quaternion has the advantages of small calculation amount, high precision, non-singularity and can work in full attitude. Quaternion is currently the most commonly used attitude representation parameter [Pittelkau, 2003]. In the field of attitude determination, it is of great practical significance to study quaternion nonlinear filtering. W.R. Hamilton introduced the concept of quaternion as early as the 19th century, but it has not been applied due to calculation conditions. Since the 1960s, with the widespread application of high-performance computers and the rapid development of aircraft posture research, quaternions have been applied. Compared with Euler angle, quaternion is not only simple to calculate, but also avoids singularity problem of Euler angle, thus achieving the ability of mobile robot to work in full posture. Quaternions are widely used in Extended Kalman filtering. Vathsal et al. [Vathsal. 1987] proposed a second-order EKF algorithm based on quaternions, calculated second-order accuracy of Taylor expansion, and improved the accuracy of pose estimation, but the amount of calculation is also increased. Crassidis et al. proposed an unscented Kalman filter algorithm for aircraft attitude determination based on conversion between modified Rodriguez parameters and attitude quaternion [Crassidis, Markley, 2003]. The algorithm effectively improves the accuracy of pose estimation through the mutual conversion between parameters, but this mutual conversion undoubtedly increases the complexity of the algorithm. This paper presents a Quaternion Square Root UKF (Quaternion Square-root UKF, Q-UKF) algorithm. The algorithm uses the Lagrange cost function, transforming the quaternion mean problem into extreme value problem of cost function. Multiplicative error quaternion is used to represent the distance between quaternion point and mean point, which avoids large calculation burden of singular value decomposition and the tedious calculation steps of the eigenvector method. In addition, the algorithm only takes quaternion vector part as pose variable, while scalar part is calculated by the unit quaternion constraint. This solves the problem of disturbing the selection of quaternion Sigma points, and also reduces the dimension of filtering state, reducing the amount of filtering operations. At the same time, combining square root unscented Kalman filter and quaternion introduced earlier in the article greatly increases the stability of the operation value. Next, we will introduce the quaternion square root unscented Kalman filter algorithm.
Selected state variable x= [ , ] ，Where ρ is the vector part of the pose quaternion and β is the gyro drift 4.1 Initialization:

Time update
The Sigma point avoids the square root operation of the state error variance matrix: The state Sigma point is divided into a gesture part and a gyro drift part: Considering passing by the quaternion in the filter update process, so here the Sigma point is selected as the quaternion point: Gyro drift Sigma point is selected as: Time update is also divided into quaternion part and nonquaternion part. The formula for calculating the quaternion partial time is： Since the gyro drift is a linear transfer process, its time is updated to: Like the mean calculation, the quaternion variance calculation also needs to be treated as a rotation vector. Here, the multiplicative error quaternion δχ | −1 ( ) is used to represent the state prediction value χ | −1 ( ) to the predicted mean ̂| −1 . Then the state prediction error variance matrix S | −1 of the quaternion vector part is: (77) δχ | −1 is the vector part of the prediction error quaternion.

Measurement update
Similar to the Sigma point selection of time update, the disturbance Sigma point is selected as： Divide it into quaternion and gyro drift： Sigma part of the quaternion in the measurement update is selected as； Gyro drift part Sigma in measurement update is selected as Quaternion measurement is updated to： Quaternion measurement error covariance matrix and state measurement error variance matrix are:

States Enhancement：
From above, state enhancement matrix is： State error vector is updated to： Thus the quaternion state update amount and gyro drift update to: State error variance matrix is: =chol( , −1 , , −1)

EXPERIMENTAL RESULTS AND ANALYZES
This paper utilizes MATLAB platform for mobile robot SLAM simulation experiment, and EKF, SR-UKF and QSR-UKF are simulated respectively. First create an experimental environment based on point features, with a size of 80m X 80m, a total of 15 navigation marks, and a number of randomly set point features.
The sampling time of Lidar is 0.2s, maximum detection distance is 30m, speed of robot during the motion v = 4m / s, process noise covariance matrix Q is [1.9 0 0; 0 1.2 0; 0 0 0.8], covariance matrix of observation noise R is [0.5 0; 0 0.1]. During the experiment, the trajectory diagrams of the robot running with EKF, SR-UKF and QSR-UKF are displayed. Among them, the solid line represents the true trajectory of the robot movement, and the dotted line represents the path estimation of the robot planning. "·" And "+" indicate the actual position and estimated position of the environmental feature. It can be seen from the chart that the estimation accuracy of the two improved algorithms is higher than that of the original EKF-SLAM. based on the EKF-SLAM filtering method, when the covariance matrix is small, that is, the process noise is small, the accuracy of the EKF-SLAM algorithm is not much different from that of SR-UKF-SLAM and QSR-UKF-SLAM. By adjusting the covariance matrix and increasing the noise error, it is obvious that SR-UKF and QSR-UKF-SLAM have higher accuracy in pose estimation. At the same time, it can be found in the table that QSR-UKF improves the stability of the robot's movement process, and the specific performance is better than EKF-SLAM and SR-UKF-SLAM  The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLIII-B4-2020, 2020 XXIV ISPRS Congress (2020 edition)

CONCLUSION
This paper presents a better solution to the nonlinear optimization problem of mobile robot SLAM. This paper first introduces and analyses the algorithms of extended Kalman filtering and unscented Kalman filtering, and proposes the limitations of these two algorithms. Aiming at these problems, corresponding solutions are given. This paper proposes to add the square root form of Sigma points to the unscented Kalman filter to increase its stability and estimation accuracy. Because the quaternion has the advantages of small calculation amount and high precision, this paper proposes a method of calculating the quaternion mean based on the Lagrangian cost function method, and only takes the vector part of the quaternion as the pose variable. Solve the problem of disturbing the selection of quaternion Sigma points, and also reduce the dimension of the filtering state, reducing the amount of filtering operations as well.
To this end, this paper combines the Square Root UKF algorithm and proposes Quaternion Square Root UKF algorithm. Simulation experiment results show that, compared with EKF and SR-UKF algorithms, QSR-UKF algorithm uses a relatively simple singular value decomposition and Cholesky decomposition to implement square-root operation of the error variance matrix, which makes the algorithm's numerical operation characteristics be improved to some extent and more stability as well.