Simultaneous Localization and Mapping Method Based on Improved Cubature Kalman Filter

Toward solving some of the problems of low precision, poor stability, and complex calculation in the simultaneous localization and mapping (SLAM) of mobile robots, an improved cubature Kalman filter SLAM (ICKF-SLAM) algorithm based on the cubature Kalman filter SLAM (CKF-SLAM) algorithm is proposed. Firstly, the error covariance matrix of the state vector is obtained through the motion model and observation model of the mobile robot. Then, the information matrix is obtained by the inverse operation, and the information state vector is updated in the prediction and update phases. The proposed method reduces the computational complexity and improves the accuracy of the algorithm. Simulation results show that compared with CKF-SLAM, the root mean square error of ICKF-SLAM is reduced by 11.8%.


Introduction
With the development of computer and mechanical technology, mobile robots are expected to replace humans in many areas. In an environment with insufficient information or with dynamic changes, vehicles or mobile robots can sense the environment through sensors, adjust their behaviors and actions, and complete tasks independently. The robots automatically obtain environmental information about the area of operation through sensors, locate their own positions, construct environmental models, and realize path planning. The simultaneous localization and mapping (SLAM) problem can be described as follows: (1) in an unknown environment, without prior knowledge, a mobile robot starts to move from an unknown position, locates itself during the process of moving, and draws an incremental map. The purpose of SLAM is to obtain the probability distributions of landmarks and the robot pose in an uncertain environment with sensor noise.
The Kalman filter (KF) is an algorithm that can estimate the state of a dynamic system given measurements observed over time. In many studies, the KF has been used to estimate the attitude of mobile robots and features of the local environment. (2,3) Smith and Cheeseman proposed an extended Kalman filter-based SLAM (EKF-SLAM) algorithm, but it has some problems, such as complex calculation, low filtering accuracy, poor data association, and orientation error. (4) Julier and Uhlmann proposed an unscented Kalman filter (UKF) SLAM algorithm, which improves the estimation accuracy and computational efficiency compared with EKF-SLAM, (5) but it is only suitable for low-dimensional (n ≤ 3) nonlinear systems. In the estimation process of high-dimensional systems, there will be a large error, and the adjustment of parameters will lead to inflexibility of the algorithm. A new SLAM algorithm based on a cubature Kalman filter (CKF-SLAM) was proposed without considering the evaluation of Jacobians during the prediction and update phases. (6) However, the complete noise distribution features must be known in order to obtain good results.
A double-layer CKF-SLAM algorithm was also proposed, where the inner CKF is used to calculate the prediction state of the next time, and the outer CKF is used to update the prediction state to obtain more accurate estimation, but this algorithm has the disadvantage of a large computational load. (7) It is difficult for a mobile robot to obtain the statistical parameters of its motion accurately due its mobility. A statistical feature estimation method was also proposed for an environment in which the statistical parameters of the noise are unknown. (8) In nonlinear systems, owing to the influence of time-varying observation noise, especially heavy-tailed noise, the traditional filtering algorithm easily diverges. The cubature integral method is used to solve this problem effectively. (9) Although the positioning accuracy is improved with this method, there are still some problems such as numerical instability. (10) To overcome the problems of mutation of the target state and an unknown noise covariance matrix, which reduce the performance of the traditional Gaussian approximation nonlinear filter, Wang et al. proposed a method based on a Bayesian strong tracking interpolation CKF (VB-STICKF), using an interpolation method to calculate the volume. Although this method has good performance for a moving target, it has high computational complexity. (11) To overcome the shortcomings of the above algorithms, we proposed an improved cubature Kalman filter (ICKF) algorithm, which takes the inverse of the covariance matrix to obtain the information matrix, then updates the information state vector in the prediction and update phases. The proposed method extends the application range of the CKF and avoids the calculation of the measurement covariance matrix and Kalman gain in the algorithm update phase, resulting in higher estimation accuracy. The average error is decreased by 11.8% compared with that of the CKF.
The main contents of this paper are as follows. The SLAM model and the related CKF are introduced in Sects. 2 and 3, respectively. In Sect. 4, the proposed ICKF algorithm is discussed. The performances of different algorithms are evaluated through simulation in Sect. 5. Finally, conclusions are drawn in Sect. 6.

SLAM model
We suppose that a mobile robot moves in an unknown environment and uses its sensors to observe some unknown landmarks, as shown in Fig. 1, (12) where x k is the pose state vector of the mobile robot, u k is the control vector that drives the mobile robot state from time k − 1 to time k, m i is the position state vector of the ith static environment feature, and z i,k is the vector with the ith static environmental feature observed by the mobile robot at time k.

SLAM probability model
The SLAM problem can be described as using the joint probability distribution of the robot pose and the position at time k to observe the probability distribution of the historical information Z 0,k and control the input historical information U 0,k and the initial pose. (13)  The joint state probability distribution at time k can be calculated using the Bayesian theorem, considering the joint state probability distribution at time k − 1 of , the observation vector z k , and the control input vector u k at time k.

CKF
Owing to the drift error of the robot and the uncertainty of the motion state and the environment, the system model and noise are often unknown. The states and parameters of the system must be estimated by a suboptimal approximation method, such as the EKF, particle filter (PF), or UKF. The CKF is an advanced suboptimal filter method proposed by Haykin and Arasaratnam, which is robust against divergence in high-dimensional nonlinear systems. (14) It is based on the cubature criterion and can obtain integral calculation results with high computational efficiency and numerical precision. (15) The key concept of the CKF is to select 2n (n represents the corresponding state dimension) equal-weight cubature points ( , ) i i ξ ω to calculate the Gaussian weighted integral through the third-order spherical radial cubature rule. (16)

Cubature points
A high-dimensional integral can usually be expressed as where f(•) is a function and R n represents an integrable space. In general, it is difficult to obtain the analytical solution of Eq. (2), and an approximate solution needs to be obtained by numerical integration, which can be approximated by the third-order spherical radial cubature rule ( Julier et al. proposed a CKF nonlinear Gaussian filter based on a spherical radial criterion to select the above cubature point set ( , ) i i ω ξ , (17) which can be expressed as where [1] i represents the corresponding ith column element in the set [1].

Calculation procedures of the CKF algorithm
When the state posterior density function at time k can be obtained. The procedures of the CKF are shown in Fig. 2, which mainly consist of two steps: the prediction phase and the update phase.

Step 1. Predict phase
By Cholesky decomposition of the error covariance matrix P k−1|k−1 , we obtain Cubature points (i = 1, 2, ..., 2n) are calculated by where i ξ is obtained using Eqs. (3) and (4). The propagation cubature points are ( ) The predicted state at time k is The covariance matrix for the state predicted error at time k is Step 2. Update phase Let P k|k−1 be the covariance matrix of the state predicted error at time k. By Cholesky decomposition of P k|k−1 , we obtain Cubature points (i = 1, 2, ..., 2n) can be calculated as The propagation cubature points (i = 1, 2, ..., 2n) after observation are The updated prediction at time k is 2 , The updated covariance matrix at time k is 2 , 1 , 1 , 1 1 1 1n The cross-correlation covariance matrix at time k is 2 , 1 , 1 , 1 1 1 1ˆn The Kalman gain can be obtained as The estimated state at time k is ( ) The estimated value of the state error covariance matrix at time k is

ICKF
For a linear Gaussian system, the distributed Kalman filter (DKF) algorithm was first proposed by Olfati-Saber, (18) which combines the average consistency strategy with the KF algorithm. Its basic idea is to use the inverse of the covariance matrix to represent the KF equivalently. Since the CKF algorithm is the optimal approximate solution of the nonlinear Gaussian filter, to improve its performance, an ICKF algorithm is proposed by using the information state vector and information matrix (19) as the inverse of the state vector and the inverse of the error covariance matrix of the CKF, respectively, in the predict and update phases.
We first define the information matrix B k|k and the information state vector |k k b as The algorithm steps of the ICKF are as follows.
Step 1. Prediction phase Similar to the CKF, after obtaining 1k k x − (state prediction at time k) and the associated covariance matrix P k|k−1 ， the information matrix B k|k and information state vector |k k b can be obtained as Step 2. Update phase Similar to the CKF, the innovation ε k can be obtained as The contribution of local information can be obtained as The associated information matrix can be calculated as (7) 1 ( ) where 1 1 1 , The update information state vector is The information matrix is The state covariance matrix is Finally, the estimated state can be calculated as (29)

ICKF-SLAM
In this section, we propose the ICKF-SLAM algorithm by using the ideas of the ICKF in Sect. 4

.1. Given the observation information
, which represents the historical information of the input and measurement pair at time k − 1, the state posterior probability density function P(x k−1 |D k−1 ) and the state transition probability P(x k |x k−1 , u k−1 ) can be obtained. The steps of the ICKF-SLAM are shown in Table 1.

System model
In our experiments, the motion model describes how the pose state (x k , y k , θ k ) T of the mobile robot changes with time under the influence of control input u k and noise interference v k . The motion model is 1 1 The observation model describes the relationship between the position state of the environmental features observed by the sensor and the global pose coordinates of the robot. A polar observation coordinate system is used in this experiment, so the observation z is the polar distance ρ and direction angle φ of an observed environmental feature relative to the robot. The observation model is  , where w k is the observation noise, which is used to describe the measurement noise and model error. (x i , y i ) are the global position coordinates of the ith observed environmental feature.

Experimental parameters
Our experiment was carried out in an outdoor area of 250 × 200 m 2 with an ideal moving path and 135 landmarks. Starting from (0, 0), a mobile robot moves counterclockwise along the ideal moving path.
The pose of the robot can be described by a three-dimensional state vector (x, y, θ) T , which includes its position (x, y) and attitude angle θ in the global coordinate system. The attitude angle θ represents the movement direction of the robot, which is positive in the counterclockwise direction and negative in the clockwise direction, and its range is −180-+180°.
The initial state value of the robot is [0, 0, 0] T , with initial speed V = 3 m/s, initial steering angle G = 0°, maximum steering angle G M = 30°, maximum angular speed G R = 20 °/s, and wheelbase WB = 4 m.
Other parameters include the system sampling time ∆T = 0.025 s, speed error σ v = 0.3 m/s, steering angle error σ g = 3 °/s, system noise , a sensor sampling time of 0.2 s, and a maximum sensing distance of 30 m.

Simulation results
Using the experimental parameters in Sect. 5.2, we carried out 50 Monte Carlo simulation experiments on three SLAM algorithms, UKF-SLAM, (5) CKF-SLAM, (19) and ICKF-SLAM, taking the mean value of all the experiments as the final result. Figure 3 shows the simulation trajectories of ICKF-SLAM. Figure 4 shows the trajectories of the three SLAM algorithms, and Fig. 5 is a local zoom of Fig. 4 with x ranging from 60 to 80 m and y ranging from 0 to 60 m. Figures 6 and 7 show the estimation error of the three SLAM algorithms in the x-and y-directions, respectively. We first analyze the stability of these algorithms. For the x-direction in Fig. 6, the stability is greatest for ICKF-SLAM and the error is less than 2 m, whereas the errors of CKF-SLAM and UKF-SLAM are less than 3.5 m, and UKF-SLAM has slightly better stability than CKF-SLAM. For the y-direction in Fig. 7, UKF-SLAM has the lowest stability, and the error is less than 3 m; the stabilities of ICKF-SLAM and CKF-SLAM are almost the same, and the error is less than 2.5 m.

Analysis of estimation errors
Next, we analyze the estimated accuracy of these algorithms. It can be seen from Fig. 6 that the estimation error in the x-direction increases for these three algorithms from 6000 to 9000 steps, and Fig. 7 shows that the estimation error in the y-direction increases from 4000 to 9000 steps. ICKF-SLAM has the least estimation error in the x-direction, and its estimation error in the y-direction is between those of the other two algorithms. Although the estimation accuracy of UKF-SLAM in the x-direction is slightly better than that of CKF-SLAM in Fig. 6, its estimation accuracy is lowest in the y-direction in Fig. 7. In general, the error of UKF-SLAM is the largest, and ICKF-SLAM has the best performance in terms of both accuracy and numerical stability.       Table 2. The data in the table show that the RMSE estimated by ICKF-SLAM is the smallest and the RMSE estimated by UKF-SLAM is the largest. UKF-SLAM has the longest running time because the UKF adopts 2n + 1 sampling points, whereas ICKF-SLAM and CKF-SLAM both adopt 2n cubature points, so the average difference between the running times of CKF-SLAM and ICKF-SLAM is small.

Consistency analysis
We use the normalized estimated error squared (NEES) shown in Eq. (34) to test the consistency of the SLAM algorithms. When n experiments are carried out, the n-average NEES (mean NEES, MNEES) shown in Eq. (35) is needed for consistency analysis.
where | k k x  represents the estimation error at time k and P k|k represents the covariance matrix of the estimated error. The system noise has an approximately Gaussian distribution, ε k has a χ 2 distribution with d degrees of freedom, where d = dim(x k ), and α represents the level of significance. In our experiment, we set N = 50, d = 3, and α = 0.05, and obtain (38) Figure 8 shows the variation of MNEES for UKF-SLAM, CKF-SLAM, and ICKF-SLAM with the number of running steps. From Fig. 8, we can see that ICKF-SLAM has the greatest consistency among the three algorithms.

Conclusions
In this paper, we proposed an improved SLAM algorithm based on the CKF that uses the inverse operation of the state vector and the covariance matrix instead of the operation of the covariance matrix in the update phase of the algorithm. The proposed algorithm avoids the calculation of the Kalman gain and improves the algorithm accuracy. The consistency of the algorithm was verified by analysis.