# A SLAM Algorithm Based on Adaptive Cubature Kalman Filter

Authors: Fei Yu; Qian Sun; Chongyang Lv; Yueyang Ben; Yanwei Fu
Journal: Mathematical Problems in Engineering
 (2014)

---

## Abstract

We need to predict mathematical model of the system and a priori knowledge of the noise statistics when traditional simultaneous localization and mapping (SLAM) solutions are used. However, in many practical applications, prior statistics of the noise are unknown or time-varying, which will lead to large estimation errors or even cause divergence. In order to solve the above problem, an innovative cubature Kalman filter-based SLAM (CKF-SLAM) algorithm based on an adaptive cubature Kalman filter (ACKF) was established in this paper. The novel algorithm estimates the statistical parameters of the unknown system noise by introducing the Sage-Husa noise statistic estimator. Combining the advantages of the
CKF-SLAM and the adaptive estimator, the new ACKF-SLAM algorithm can reduce the state estimated error significantly and improve the navigation accuracy of the SLAM system effectively. The performance of this new algorithm has been examined through numerical simulations in different scenarios. The results have shown that the position error can be effectively reduced with the new adaptive CKF-SLAM algorithm. Compared with other traditional SLAM methods, the accuracy of the nonlinear SLAM system is significantly improved. It verifies that the proposed ACKF-SLAM algorithm is valid and feasible.


---

## Body

## 1. Introduction

The simultaneous localization and mapping (SLAM) is that the mobile robot builds up the environment map in the unknown environment by utilizing the sensors onboard; at the same time the robots location is computed by the same map [1–3]. The SLAM was first proposed by Smith, Self, and Cheeseman in 1987, estimating robots poses and the features of the environment simultaneously using extend Kalman filter (EKF) [4]. Since then, the SLAM has been implemented in a number of different domains from indoor robots to outdoor, underwater, ground, and airborne systems [5, 6]. The SLAM algorithm has received extensive attention.SLAM problem involves unknown and uncertain environment description and sensor noise; therefore its essence is a probabilistic estimation issue which is widely accepted by numerous researchers now [4]. At present, the most typical and widely used SLAM algorithm is EKF-based SLAM (EKF-SLAM) introduced in [7–9]. But the precision of EKF-SLAM is limited because the Taylor expansion will cause truncation errors; on the other hand the EKF needs to calculate the fussy Jacobian matrix which increases the computational load. To solve the problems of the EKF, the unscented Kalman filter (UKF) proposed by Julier and Uhlmann [10] was used in SLAM [11]. Unlike the EKF, the UKF uses a set of chosen samples to represent the state distribution. The UKF-based SLAM (UKF-SLAM) algorithm can not only avoid the calculation of the Jacobian and Hessian matrices but also obtain higher approximation accuracy with the unscented transformation (UT). However, for high-dimensional systems, the computation load is still heavy; thus, the filter converges slowly. In 2009, Arasaratnam and Haykin [12, 13] proposed a more accurate nonlinear filtering solution based on a cubature transform named the cubature Kalman filter (CKF) which can avoid linearization of the nonlinear system by using cubature point sets to approximate the mean and variance. The third-order accuracy of the system can be achieved with this method. What is more, the computation complexity of the CKF is lower than the one of the UKF. Because of its high accuracy and low calculation load, the CKF is widely used in attitude estimation and navigation [14]. CKF-SLAM was introduced in [5], which achieved high positioning accuracy compared with traditional SLAM algorithms.However, the above filters are all based on the framework of the Kalman filter (KF); it can only achieve a good performance under the assumption that the complete and exact information of the noise distribution has to be known as a prior. But in practice, the prior noise statistic is usually unknown totally or may be always time-varying because the model and the noise of system are influenced by the drift errors of the robots component, the kinematic model of the robot, and the uncertainty of the outdoor environment. With the uncertain or time-varying noise statistic, the state estimation will have large errors, or, even, the filters will be possible to diverge [15]. For SLAM, it has been shown that the performance can become significantly worse and the estimated results practical diverge with an incorrect a priori knowledge of the noise statistics [16, 17]. Therefore, to solve this problem, many researchers have proposed the H∞ filter algorithm and the adaptive filter algorithm which have been focused on in recent years. Although the H∞ filter has numerous advantages, such as being robust, independent of the noise statistic, and easy to set the filter parameters, the low filter accuracy is still its fatal flaw [18–20]. Hence, we only focus on the adaptive filter algorithm in this work.The core idea of the adaptive filter algorithm is that, while the whole system is filtering, estimating and modifying the parameters of the system model and the statistic of the system noise which are unknown or time-varying, to decrease state estimation errors and improve the estimation accuracy. There are numerous adaptive algorithms, including the distribution test method, the maximum a posteriori method, the correlation test method, and the Sage-Husa adaptive method [21, 22]. Therein, the Sage-Husa adaptive method which is suboptimal unbiased maximum a posteriori (MAP) noise estimator is one of the widely used adaptive filtering methods [23], since it has advantages that the recursive formula is simple and the principle is clear and easy to implement. Therefore we use this kind of adaptive filtering to estimate unknown system noise here.In this paper a novel adaptive filtering algorithm based on the CKF-SLAM, named adaptive CKF-SLAM (ACKF-SLAM), for mobile robot outdoors, was proposed. In this proposed algorithm, we introduced the Sage-Husa adaptive filtering method into the traditional CKF-SLAM algorithm. The system noise and the statistics of the observation noise were estimated on real time and modified to reduce the errors of the system model by using the adaptive estimator of the time-varying noise. What is more, it also prevented the filter divergence and improved the estimation accuracy significantly. The rest of the paper was organized as follows. The description of the SLAM and the traditional CKF-SLAM algorithm were presented in Section2. Section 3 showed the adaptive EKF algorithm and then a new ACKF-SLAM algorithm was proposed. Numerical examples in different scenarios along with specific analysis were given in Section 4. Section 5 concluded this paper.


## 2. SLAM

### 2.1. Description of the SLAM

The SLAM is always described as follows: the mobile robot starting in an unknown location without previous knowledge of the environment builds a map using its onboard sensors while, simultaneously, using this same map to determine the location of the robot within this map [2]. Figure 1 illustrates the characteristic data acquisition and processing of the SLAM.Figure 1
Sketch of the SLAM algorithm.The essence of the SLAM is the filtering estimation of the system in the whole path. The system states are composed of the robots pose (position, orientation) and the landmarks states observed in the environment. Let the robot states be represented by its pose with an estimated vectorXv and the covariance matrix Pvv, defined as
(1)Xv=[xvyvφv]T,Pvv=[σxvxv2σxvyv2σxvφv2σxvyv2σyvyv2σyvφv2σxvφv2σyvφv2σφvφv2].Without loss of generality, let us assume that there are a set ofn 2D static point features observed in the map; the position estimations of these features are given by their estimated vector Xm and the covariance matrix Pmm as
(2)Xm=[x1y1⋯xnyn]T,Pmm=[σx1x12σx1y12⋯σx1xn2σx1yn2σx1y12σy1y12⋯σy1xn2σy1yn2⋮⋮⋱⋮⋮σx1xn2σy1xn2⋯σxnxn2σxnyn2σx1yn2σy1yn2⋯σxnyn2σynyn2].In the SLAM, the total state vectorX is composed of the robots states Xv and the landmarks states Xm. Hence the estimations of the total state vector X and the corresponding total error covariance matrix P are given as
(3)X=[XvTXmT]T,P=[PvvPvmPvmTPmm],
wherein Pvm denotes the robot-map correlation.The key of the SLAM is to determine the posterior probability density of the state vectorX which is p(Xv,k,Xm∣uk,zk) and the implication is to obtain the joint probability density of the robot pose Xv,k and the map Xm when the control input uk and observation zk are known. We can obtain the following equation by utilizing the Bayes formula:
(4)p(Xv,k,Xm∣uk,zk)=p(zk∣Xv,k,Xm)×∫‍p(Xv,k∣Xv,k-1,uk)hh×p(Xv,k-1,Xm∣uk-1,zk-1)dXv,k-1,
wherein uk is the input control of the robot at time k which drives from pose Xv,k-1 at time k-1 to pose Xv,k at time k; zk is observation to the landmark at time k; p(Xv,k∣Xv,k-1,uk) is the motion model which is the conditional probability of Xv,k at time k when uk and Xv,k-1 are known; p(zk∣Xv,k,Xm) is the observation model which is the conditional probability of zk at time k when Xv,k and the landmark collection matrix Xm are known.


### 2.2. CKF-SLAM

In this subsection the principle of the classical CKF is introduced. Consider the general discrete-time nonlinear state-space model as follows:(5)xk=f(xk-1)+Wk-1,zk=h(xk)+ηk,
wherein xk and zk are the state vector and the measurement vector at time k, respectively; f(·) and h(·) are specific known nonlinear functions; and Wk-1 and ηk are the noise vectors from two independent Gaussian processes with their means being qk-1 and rk and their covariance matrices being Qk-1 and Rk, respectively. The CKF is proposed to solve this nonlinear filtering problem on the basis of the spherical-radial cubature criterion. Firstly it approximates the mean and variance of the probability distribution through a set of 2N (N is the dimension of the nonlinear system) cubature points with the same weight, propagates the above cubature points through the nonlinear functions, and then calculates the mean and variance of the current approximate Gaussian distribution by the propagated cubature points [12]. A set of 2N cubature points are given by [ξi,ωi], where ξi is the ith cubature point and ωi is the corresponding weight:
(6)ξi=N[1]i,ωi=12N,
wherein i=1,2,…,2N. Under the assumption that the posterior density at time k-1 is known, the steps involved in the time-update and the measurement-update of the CKF are summarized as follows [12].Time-update:(1)
factorize(7)Pk-1∣k-1=Sk-1∣k-1Sk-1∣k-1T,(2)
evaluate the cubature points(i=1,2,…,2N)(8)Xi,k-1∣k-1=Sk-1∣k-1ξi+x^k-1,(3)
evaluate the propagated cubature points(9)Xi,k∣k-1*=f(Xi,k-1∣k-1)+qk-1,(4)
estimate the predicted state(10)x^k∣k-1=12N∑i=12N‍Xi,k∣k-1*,(5)
estimate the predicted error covariance(11)Pk∣k-1=12N∑i=12N‍Xi,k∣k-1*Xi,k∣k-1*T-x^k∣k-1x^k∣k-1T+Qk-1.Measurement-update:(1)
factorize(12)Pk∣k-1=Sk∣k-1Sk∣k-1T,(2)
evaluate the cubature points(13)Xi,k∣k-1=Sk∣k-1ξi+x^k∣k-1,(3)
evaluate the propagated cubature points(14)Yi,k∣k-1=h(Xi,k∣k-1)+rk,(4)
estimate the predicted measurement(15)y^k∣k-1=12N∑i=12N‍Yi,k∣k-1,(5)
estimate the innovation covariance matrix(16)Pk∣k-1zz=12N∑i=12N‍Yi,k∣k-1Yi,k∣k-1T-y^k∣k-1y^k∣k-1T+Rk,(6)
estimate the cross-covariance matrix(17)Pk∣k-1xz=12N∑i=12N‍Xi,k∣k-1Yi,k∣k-1T-x^k∣k-1y^k∣k-1T.With the new measurement vectorzk, the estimation of the state vector x^k∣k and its covariance matrix Pk∣k at time k can be obtained as follows.(1)
Estimate the Kalman gain(18)Kk=Pk∣k-1xz(Pk∣k-1zz)-1.(2)
Estimate the updated state(19)x^k=x^k∣k-1+Kk(zk-z^k∣k-1).(3)
Estimate the corresponding error covariance(20)Pk∣k=Pk∣k-1-KkPk∣k-1zzKkT.The CKF-SLAM algorithm uses cubature rule and2N cubature point sets [ξi,ωi] to compute the mean and variance of the probability distribution without any linearization of the SLAM system. Thus, the CKF-SLAM algorithm does not demand to calculate Jacobian and Hessian matrices so that the truncation errors can be avoided. Hence, the estimation accuracy can reach the third order or higher. Furthermore, the computational complexity is alleviated to a certain extent than the UKF [13].


## 2.1. Description of the SLAM

The SLAM is always described as follows: the mobile robot starting in an unknown location without previous knowledge of the environment builds a map using its onboard sensors while, simultaneously, using this same map to determine the location of the robot within this map [2]. Figure 1 illustrates the characteristic data acquisition and processing of the SLAM.Figure 1
Sketch of the SLAM algorithm.The essence of the SLAM is the filtering estimation of the system in the whole path. The system states are composed of the robots pose (position, orientation) and the landmarks states observed in the environment. Let the robot states be represented by its pose with an estimated vectorXv and the covariance matrix Pvv, defined as
(1)Xv=[xvyvφv]T,Pvv=[σxvxv2σxvyv2σxvφv2σxvyv2σyvyv2σyvφv2σxvφv2σyvφv2σφvφv2].Without loss of generality, let us assume that there are a set ofn 2D static point features observed in the map; the position estimations of these features are given by their estimated vector Xm and the covariance matrix Pmm as
(2)Xm=[x1y1⋯xnyn]T,Pmm=[σx1x12σx1y12⋯σx1xn2σx1yn2σx1y12σy1y12⋯σy1xn2σy1yn2⋮⋮⋱⋮⋮σx1xn2σy1xn2⋯σxnxn2σxnyn2σx1yn2σy1yn2⋯σxnyn2σynyn2].In the SLAM, the total state vectorX is composed of the robots states Xv and the landmarks states Xm. Hence the estimations of the total state vector X and the corresponding total error covariance matrix P are given as
(3)X=[XvTXmT]T,P=[PvvPvmPvmTPmm],
wherein Pvm denotes the robot-map correlation.The key of the SLAM is to determine the posterior probability density of the state vectorX which is p(Xv,k,Xm∣uk,zk) and the implication is to obtain the joint probability density of the robot pose Xv,k and the map Xm when the control input uk and observation zk are known. We can obtain the following equation by utilizing the Bayes formula:
(4)p(Xv,k,Xm∣uk,zk)=p(zk∣Xv,k,Xm)×∫‍p(Xv,k∣Xv,k-1,uk)hh×p(Xv,k-1,Xm∣uk-1,zk-1)dXv,k-1,
wherein uk is the input control of the robot at time k which drives from pose Xv,k-1 at time k-1 to pose Xv,k at time k; zk is observation to the landmark at time k; p(Xv,k∣Xv,k-1,uk) is the motion model which is the conditional probability of Xv,k at time k when uk and Xv,k-1 are known; p(zk∣Xv,k,Xm) is the observation model which is the conditional probability of zk at time k when Xv,k and the landmark collection matrix Xm are known.


## 2.2. CKF-SLAM

In this subsection the principle of the classical CKF is introduced. Consider the general discrete-time nonlinear state-space model as follows:(5)xk=f(xk-1)+Wk-1,zk=h(xk)+ηk,
wherein xk and zk are the state vector and the measurement vector at time k, respectively; f(·) and h(·) are specific known nonlinear functions; and Wk-1 and ηk are the noise vectors from two independent Gaussian processes with their means being qk-1 and rk and their covariance matrices being Qk-1 and Rk, respectively. The CKF is proposed to solve this nonlinear filtering problem on the basis of the spherical-radial cubature criterion. Firstly it approximates the mean and variance of the probability distribution through a set of 2N (N is the dimension of the nonlinear system) cubature points with the same weight, propagates the above cubature points through the nonlinear functions, and then calculates the mean and variance of the current approximate Gaussian distribution by the propagated cubature points [12]. A set of 2N cubature points are given by [ξi,ωi], where ξi is the ith cubature point and ωi is the corresponding weight:
(6)ξi=N[1]i,ωi=12N,
wherein i=1,2,…,2N. Under the assumption that the posterior density at time k-1 is known, the steps involved in the time-update and the measurement-update of the CKF are summarized as follows [12].Time-update:(1)
factorize(7)Pk-1∣k-1=Sk-1∣k-1Sk-1∣k-1T,(2)
evaluate the cubature points(i=1,2,…,2N)(8)Xi,k-1∣k-1=Sk-1∣k-1ξi+x^k-1,(3)
evaluate the propagated cubature points(9)Xi,k∣k-1*=f(Xi,k-1∣k-1)+qk-1,(4)
estimate the predicted state(10)x^k∣k-1=12N∑i=12N‍Xi,k∣k-1*,(5)
estimate the predicted error covariance(11)Pk∣k-1=12N∑i=12N‍Xi,k∣k-1*Xi,k∣k-1*T-x^k∣k-1x^k∣k-1T+Qk-1.Measurement-update:(1)
factorize(12)Pk∣k-1=Sk∣k-1Sk∣k-1T,(2)
evaluate the cubature points(13)Xi,k∣k-1=Sk∣k-1ξi+x^k∣k-1,(3)
evaluate the propagated cubature points(14)Yi,k∣k-1=h(Xi,k∣k-1)+rk,(4)
estimate the predicted measurement(15)y^k∣k-1=12N∑i=12N‍Yi,k∣k-1,(5)
estimate the innovation covariance matrix(16)Pk∣k-1zz=12N∑i=12N‍Yi,k∣k-1Yi,k∣k-1T-y^k∣k-1y^k∣k-1T+Rk,(6)
estimate the cross-covariance matrix(17)Pk∣k-1xz=12N∑i=12N‍Xi,k∣k-1Yi,k∣k-1T-x^k∣k-1y^k∣k-1T.With the new measurement vectorzk, the estimation of the state vector x^k∣k and its covariance matrix Pk∣k at time k can be obtained as follows.(1)
Estimate the Kalman gain(18)Kk=Pk∣k-1xz(Pk∣k-1zz)-1.(2)
Estimate the updated state(19)x^k=x^k∣k-1+Kk(zk-z^k∣k-1).(3)
Estimate the corresponding error covariance(20)Pk∣k=Pk∣k-1-KkPk∣k-1zzKkT.The CKF-SLAM algorithm uses cubature rule and2N cubature point sets [ξi,ωi] to compute the mean and variance of the probability distribution without any linearization of the SLAM system. Thus, the CKF-SLAM algorithm does not demand to calculate Jacobian and Hessian matrices so that the truncation errors can be avoided. Hence, the estimation accuracy can reach the third order or higher. Furthermore, the computational complexity is alleviated to a certain extent than the UKF [13].


## 3. SLAM Algorithm Based on Adaptive CKF

### 3.1. Adaptive EKF

When the system noises are unknown or time-varying, the filtering algorithm cannot be recursively carried out in a common way. One effective solution is the adaptive filtering algorithm. The adaptive filtering technology has become a focus of the research attempting to solve the filter divergence problem caused by the inaccurate statistical properties of the noise and the mathematical model itself. The Sage-Husa adaptive filtering proposed by Sage and Husa is one of the most widely used adaptive filtering algorithms. Estimating the statistical parameters of the virtual noises online, along with the recursive estimate of the state, this method can reduce the error of the model and improve the accuracy of the whole system validly.Nowadays, the adaptive EKF based on Sage-Husa is the most widely used adaptive algorithm. However, the estimated accuracy is low because of the truncation errors of the Taylor expansion in the EKF. Considering the advantages of the CKF, an adaptive CKF algorithm was proposed to improve the estimated accuracy in this work on the basis of the adaptive EKF. First of all, the adaptive EKF was introduced in detail here. Suppose that the discrete state equation and the observation equation of the nonlinear system are shown as (5). On the basis of [24], the Sage-Husa estimator based adaptive EKF algorithm is shown as
(21)x^k∣k-1=f(x^k-1)+q^k-1,Pk∣k-1=Fk-1Pk-1Fk-1T+Q^k-1,ξk=zk-h(x^k∣k-1)-r^k,Sk=HkPk∣k-1HkT+R^k,Kk=Pk∣k-1HkTSk-1,x^k=x^k∣k-1+Kkξk,Pk=(I-KkHk)Pk∣k-1,
wherein Pk is the error covariance matrix of the state x^k, Kk is the filter gain matrix, Fk-1 and Hk are the nonlinear states function f(·) and observation function h(·) with respect to states, respectively, ξk is the difference between the measurement and the prediction, and Sk covariance matrix of the ξk, q^k-1, r^k, Q^k-1, and R^k is obtained by recurrence of the time-varying noise statistics estimator:
(22)q^k=(1-dk-1)q^k-1+dk-1(x^k-f(x^k-1)),Q^k=(1-dk-1)Q^k-1+dk-1[KkξkξkTKkT+Pk-Fk-1Pk-1Fk-1T],r^k=(1-dk-1)r^k-1+dk-1(zk-h(X^k∣k-1)),R^k=(1-dk-1)R^k-1+dk-1[ξkξkT-HkPk∣k-1HkT],dk-1=(1-b)(1-bk),
wherein ξk+1 denotes the innovation sequence; b(0<b<1) denotes the forgetting factor whose value is often set between 0.95 and 0.99. The memory span is limited utilizing the forgetting factor. As a result, the old information is forgotten little by little and the new information plays the lead role in estimating.It has been analyzed that adaptive filtering algorithms cannot estimate the process and the measurement noise simultaneously in [25]. However, theoretical derivations and simulations in [26] show that when the measurement noise is already known, the process noise can be obtained by the iteration. Usually, the measurement noise statistic is relatively well known compared to the system model noise. So we can use this adaptive estimator to estimate the system noise and enhance the filtering accuracy.


### 3.2. Adaptive CKF

It is well known that the truncation errors of the Taylor expansion in the EKF will reduce the estimated accuracy or even diverge the filter. However, the new algorithm named the CKF can increase the estimated accuracy effectively, which was referred to in Section2.2. So analogous with the adaptive EKF, the Sage-Husa noise statistics estimator was embedded into the CKF-SLAM. And the adaptive CKF-SLAM algorithm was obtained, which combines the advantages of the CKF-SLAM and the Sage-Husa adaptive method. Without loss of generality, we still consider the nonlinear system shown as (5) and the detailed algorithm of the ACKF-SLAM is given as follows.Time-update:(23)Pk-1∣k-1=Sk-1∣k-1Sk-1∣k-1T,Xi,k-1∣k-1=Sk-1∣k-1ξi+x^k-1,Xi,k∣k-1*=f(Xi,k-1∣k-1)+q^k-1,x^k∣k-1=12N∑i=12N‍Xi,k∣k-1*,Pk∣k-1=12N∑i=12N‍Xi,k∣k-1*Xi,k∣k-1*T-x^k∣k-1x^k∣k-1T+Q^k-1.Measurement-update:(24)Pk∣k-1=Sk∣k-1Sk∣k-1T,Xi,k∣k-1=Sk∣k-1ξi+x^k∣k-1,Yi,k∣k-1=h(Xi,k∣k-1)+r^k,z^k∣k-1=12N∑i=12N‍Yi,k∣k-1,Pk∣k-1zz=12N∑i=12N‍Yi,k∣k-1Yi,k|k-1T-z^k∣k-1z^k∣k-1T+R^k,Pk∣k-1xz=12N∑i=12N‍Xi,k∣k-1Yi,k∣k-1T-x^k∣k-1z^k∣k-1T.With the new measurement vectorzk, the estimation of the state vector x^k∣k and its covariance matrix Pk∣k at time k can be obtained by the following equations:
(25)Kk=Pk∣k-1xz(Pk∣k-1zz)-1,x^k=x^k∣k-1+Kkξk,ξk=zk-z^k∣k-1,Pk∣k=Pk∣k-1-KkPk∣k-1zzKkT,q^k=(1-dk-1)q^k-1+dk-1(X^k-x^k∣k-1),Q^k=(1-dk-1)Q^k-1+dk-1[KkξkξkTKkT+Pk-Pk∣k-1],r^k=(1-dk-1)r^k-1+dk-1(zk-z^k∣k-1),R^k=(1-dk-1)R^k-1+dk-1[ξkξkT-Pk∣k-1zz],dk-1=(1-b)(1-bk).The improved ACKF-SLAM algorithm proposed in this paper can be used in the situation when the noise statistical character of the system is absolutely or approximately unknown to make sure the filter works well, enhancing the stability of the filter.


## 3.1. Adaptive EKF

When the system noises are unknown or time-varying, the filtering algorithm cannot be recursively carried out in a common way. One effective solution is the adaptive filtering algorithm. The adaptive filtering technology has become a focus of the research attempting to solve the filter divergence problem caused by the inaccurate statistical properties of the noise and the mathematical model itself. The Sage-Husa adaptive filtering proposed by Sage and Husa is one of the most widely used adaptive filtering algorithms. Estimating the statistical parameters of the virtual noises online, along with the recursive estimate of the state, this method can reduce the error of the model and improve the accuracy of the whole system validly.Nowadays, the adaptive EKF based on Sage-Husa is the most widely used adaptive algorithm. However, the estimated accuracy is low because of the truncation errors of the Taylor expansion in the EKF. Considering the advantages of the CKF, an adaptive CKF algorithm was proposed to improve the estimated accuracy in this work on the basis of the adaptive EKF. First of all, the adaptive EKF was introduced in detail here. Suppose that the discrete state equation and the observation equation of the nonlinear system are shown as (5). On the basis of [24], the Sage-Husa estimator based adaptive EKF algorithm is shown as
(21)x^k∣k-1=f(x^k-1)+q^k-1,Pk∣k-1=Fk-1Pk-1Fk-1T+Q^k-1,ξk=zk-h(x^k∣k-1)-r^k,Sk=HkPk∣k-1HkT+R^k,Kk=Pk∣k-1HkTSk-1,x^k=x^k∣k-1+Kkξk,Pk=(I-KkHk)Pk∣k-1,
wherein Pk is the error covariance matrix of the state x^k, Kk is the filter gain matrix, Fk-1 and Hk are the nonlinear states function f(·) and observation function h(·) with respect to states, respectively, ξk is the difference between the measurement and the prediction, and Sk covariance matrix of the ξk, q^k-1, r^k, Q^k-1, and R^k is obtained by recurrence of the time-varying noise statistics estimator:
(22)q^k=(1-dk-1)q^k-1+dk-1(x^k-f(x^k-1)),Q^k=(1-dk-1)Q^k-1+dk-1[KkξkξkTKkT+Pk-Fk-1Pk-1Fk-1T],r^k=(1-dk-1)r^k-1+dk-1(zk-h(X^k∣k-1)),R^k=(1-dk-1)R^k-1+dk-1[ξkξkT-HkPk∣k-1HkT],dk-1=(1-b)(1-bk),
wherein ξk+1 denotes the innovation sequence; b(0<b<1) denotes the forgetting factor whose value is often set between 0.95 and 0.99. The memory span is limited utilizing the forgetting factor. As a result, the old information is forgotten little by little and the new information plays the lead role in estimating.It has been analyzed that adaptive filtering algorithms cannot estimate the process and the measurement noise simultaneously in [25]. However, theoretical derivations and simulations in [26] show that when the measurement noise is already known, the process noise can be obtained by the iteration. Usually, the measurement noise statistic is relatively well known compared to the system model noise. So we can use this adaptive estimator to estimate the system noise and enhance the filtering accuracy.


## 3.2. Adaptive CKF

It is well known that the truncation errors of the Taylor expansion in the EKF will reduce the estimated accuracy or even diverge the filter. However, the new algorithm named the CKF can increase the estimated accuracy effectively, which was referred to in Section2.2. So analogous with the adaptive EKF, the Sage-Husa noise statistics estimator was embedded into the CKF-SLAM. And the adaptive CKF-SLAM algorithm was obtained, which combines the advantages of the CKF-SLAM and the Sage-Husa adaptive method. Without loss of generality, we still consider the nonlinear system shown as (5) and the detailed algorithm of the ACKF-SLAM is given as follows.Time-update:(23)Pk-1∣k-1=Sk-1∣k-1Sk-1∣k-1T,Xi,k-1∣k-1=Sk-1∣k-1ξi+x^k-1,Xi,k∣k-1*=f(Xi,k-1∣k-1)+q^k-1,x^k∣k-1=12N∑i=12N‍Xi,k∣k-1*,Pk∣k-1=12N∑i=12N‍Xi,k∣k-1*Xi,k∣k-1*T-x^k∣k-1x^k∣k-1T+Q^k-1.Measurement-update:(24)Pk∣k-1=Sk∣k-1Sk∣k-1T,Xi,k∣k-1=Sk∣k-1ξi+x^k∣k-1,Yi,k∣k-1=h(Xi,k∣k-1)+r^k,z^k∣k-1=12N∑i=12N‍Yi,k∣k-1,Pk∣k-1zz=12N∑i=12N‍Yi,k∣k-1Yi,k|k-1T-z^k∣k-1z^k∣k-1T+R^k,Pk∣k-1xz=12N∑i=12N‍Xi,k∣k-1Yi,k∣k-1T-x^k∣k-1z^k∣k-1T.With the new measurement vectorzk, the estimation of the state vector x^k∣k and its covariance matrix Pk∣k at time k can be obtained by the following equations:
(25)Kk=Pk∣k-1xz(Pk∣k-1zz)-1,x^k=x^k∣k-1+Kkξk,ξk=zk-z^k∣k-1,Pk∣k=Pk∣k-1-KkPk∣k-1zzKkT,q^k=(1-dk-1)q^k-1+dk-1(X^k-x^k∣k-1),Q^k=(1-dk-1)Q^k-1+dk-1[KkξkξkTKkT+Pk-Pk∣k-1],r^k=(1-dk-1)r^k-1+dk-1(zk-z^k∣k-1),R^k=(1-dk-1)R^k-1+dk-1[ξkξkT-Pk∣k-1zz],dk-1=(1-b)(1-bk).The improved ACKF-SLAM algorithm proposed in this paper can be used in the situation when the noise statistical character of the system is absolutely or approximately unknown to make sure the filter works well, enhancing the stability of the filter.


## 4. Experiments and Analysis

### 4.1. Experiment Modeling

In order to verify the effectiveness and feasibility of the new SLAM algorithm proposed in this paper, a large number of simulations are tested with the simulation environment issued by Tim Bailey from University of Sydney. The simulation environment which is a 250 m × 200 m outdoor area includes the movement path and 135 static landmark points. The mobile robot moves along the path from (0, 0) anticlockwise as it is shown in Figure2.Figure 2
Sketch of the simulation environment.The motion model of the mobile robot is shown as follows:(26)xv,k+1=[xvx,k+ΔTvk+1cos⁡(xvφ,k+θk+1)xvy,k+ΔTvk+1cos⁡(xvφ,k+θk+1)xvφ,k+ΔTvk+1sinθk+1WB]+Wk+1,
wherein [xvx,k,xvy,k,xvφ,k]T denotes the pose of the mobile robot at time k; xv,k+1 denotes the pose of the mobile robot at time k-1; ΔT denotes the sampling time; vk+1 denotes the velocity of the mobile robot at time k-1; θk+1 denotes the azimuth angle at time k-1; WB denotes the wheel base between the two axes; Wk+1 denotes the error of the system at time k-1 and W~N(0,Q). The observation model is shown as follows:
(27)zk=[riφi]=[(xi-xvx,k)2+(yi-xvy,k)2arctanyi-xvy,kxi-xvx,k-xvφ,k]+Vk,
wherein (xi,yi) denotes the detected position corresponding to the ith feature; ri denotes the distance between the ith detected feature and the mobile robot; φi denotes the distance between the ith detected feature and the heading of the mobile robot; Vk denotes the measurement error at time k and V~N(0,R).The experiment parameters are set as follows. The initial states of the mobile robotxv,0=[0,0,0]T; the sampling interval ΔT=0.025s; the velocity v=3m/s; the velocity error σv=0.25m/s; the azimuth error σθ=2°; the maximum angular rate is 0.2s; the maximum distance of measurement is 30m; the distance error σr=0.2m; the angular error σφ=1°.


### 4.2. Experimental Results and Analysis

Under the above conditions, fifty Monte Carlo simulations were performed for four SLAM algorithms, including the EKF-SLAM, CKF-SLAM, adaptive EKF-SLAM (AEKF-SLAM), and adaptive CKF-SLAM (ACKF-SLAM). And then the root mean square errors (RMSE) of the estimated results were compared.Figures3 and 4 show the comparisons of the EKF-SLAM and the CKF-SLAM; Figures 5 and 6 show the comparisons of the AEKF-SLAM and the ACKF-SLAM, wherein Figures 4 and 6 are partial enlargements of the rectangle regions in Figures 3 and 5, respectively. System noise Q and observation noise R are shown as follows:
(28)Q=[(0.25)200(3π180)2],R=[(0.1)200(π180)2].Figure 3
Comparisons of the simulation results with EKF-SLAM and CKF-SLAM.Figure 4
Partial enlargement of Figure3.Figure 5
Comparisons of the simulation results with AEKF-SLAM and ACKF-SLAM.Figure 6
Partial enlargement of Figure5.From Figures3 to 6, we obviously know that the estimated error of the ACKF-SLAM is much smaller than that of the CKF-SLAM and EKF-SLAM. It means that the ACKF-SLAM algorithm can provide higher estimated accuracy of the nonlinear SLAM system than the other three solutions.Figures7 and 8 show the RMSEs of the estimated position errors of the ACKF-SLAM, AEKF-SLAM, CKF-SLAM, and EKF-SLAM.Figure 7
Comparisons of the RMSEs of thex-axis estimated position errors.Figure 8
Comparisons of the RMSEs of they-axis estimated position errors.From the results shown in Figures7 and 8, we learn that the RMSE values of the position estimation errors of the ACKF-SLAM algorithm are within 3 m. The precision of AEKF-SLAM is nearly the same as CKF-SLAM, and both of them are lower than ACKF-SLAM algorithm. The RMSE values of the AEKF-SLAM are within 8 m while the corresponding RMSEs of the CKF-SLAM are within 10 m. The EKF-SLAM algorithm has the lowest estimation precision and the RMSEs are within 17 m. So the precision of ACKF-SLAM is higher than the other three algorithms. Comparing between these four algorithms, the estimation precisions of the standard CKF-SLAM and ACKF-SLAM are higher than those of standard EKF-SLAM and AEKF-SLAM because the cubature points are used in the CKF instead of the nonlinear transformation in the EKF. As a result, the errors can be reduced and the filtering precisions can be enhanced availably. We also can see that the estimation errors increase rapidly in the last part of the standard EKF-SLAM and CKF-SLAM. However, the estimation results of the ACKF-SLAM and AEKF-SLAM always keep stable. It is because the system process noise and observation noise are estimated and modified on real time to enhance the filter precision using the adaptive algorithm. These coincide with the theoretical analysis totally.Table1 shows the run time of the four algorithms. The calculated cost of the EKF-SLAM is the least one, the ones of the AEKF-SLAM and the CKF-SLAM are almost equal, and the one of the ACKF-SLAM is the most. That is because of the fact that the EKF only uses the Taylor expansion to linearize the nonlinear system which is extremely simple. But the CKF algorithm approximates the mean and variance by a set of 2N cubature points, so it costs a longer time than the EKF. As the adaptive algorithm is adopted in adaptive CKF-SLAM algorithm, it costs much longer time than the CKF-SLAM algorithm.Table 1
Comparisons of the run time.
SLAM algorithm
Run time (seconds)
Standard EKF-SLAM
196.18
Standard CKF-SLAM
278.52
Adaptive EKF-SLAM
231.74
Adaptive CKF-SLAM
344.39In order to verify the validity of the ACKF-SLAM sufficiently, some different simulations were done with different environment noise. Suppose that the observation noise obeys the mixture Gauss distribution asVk~0.5N(0,R1)+0.5N(0,R2), wherein
(29)R1=[(0.1)20.01π1800.01π180(π180)2],R2=[(0.01)20.3π1800.3π180(3π180)2].Figure9 illustrates the estimated errors of the ACKF-SLAM and the AEKF-SLAM algorithms and Figure 10 is the partial enlargement of the rectangle region in Figure 9. As can be seen from Figures 9 to 11, the mobile robot motion trajectory still can be estimated with a high precision by the ACKF-SLAM even though different environment noise is added.Figure 9
Comparisons of simulation results about the AEKF-SLAM and ACKF-SLAM.Figure 10
Partial enlargement of Figure9.RMSEs of position errors with the AEKF-SLAM and ACKF-SLAM algorithms.
(a)
(b)To verify the effectiveness and superiority of the new adaptive SLAM algorithm, when the system noises are time-varying, another simulation scenario was set in which the process and the observation noises are time-varying. On the basis of [2], the parameters are set as Table 2.Table 2
Simulation parameters.
Calculation step
Q
k
R
k
1
<
t
<
3000
diag
⁡
(
[
(
0.1
)
2
;
(
0.5
π
/
180
)
2
]
)
diag
⁡
(
[
(
0
.
5
)
2
;
(
0
.
2
π
/
180
)
2
]
)
3000
≤
t
<
6000
diag
⁡
(
[
(
0
.
5
)
2
;
(
2
π
/
180
)
2
]
)
diag
⁡
(
[
(
0
.
2
)
2
;
(
π
/
180
)
2
]
)
6000
≤
t
diag
⁡
(
[
(
2
)
2
;
(
4
π
/
180
)
2
]
)
diag
⁡
(
[
1
2
;
(
2
π
/
180
)
2
]
)Figure12 illustrates the estimation errors of the ACKF-SLAM, AEKF-SLAM, and CKF-SLAM algorithms when the system noises are time-varying and Figure 13 is the partial enlargement of the rectangle region in Figure 12. Figure 14 is the RMSEs of the position errors correspondingly. As can be seen from Figures 12 to 14, the estimated errors of the CKF-SLAM are the biggest and the ones of the ACKF-SLAM and the AEKF-SLAM are much smaller. That is because of the fact that when the system noises are time-varying, standard CKF-SLAM cannot accurately estimate them. However, the adaptive CKF-SLAM and adaptive EKF-SLAM can estimate and modify the parameters of the system model and the statistic of the system noises online to decrease the estimated errors and improve the accuracy effectively, owing to the Sage-Husa adaptive estimator. Additionally, comparing between the two adaptive methods, the estimated results of the ACKF-SLAM are much better than that of the AEKF-SLAM.Figure 12
Comparisons of simulation results with time-varying noises.Figure 13
Partial enlargement of Figure12.RMSEs of position errors of the CKF-SLAM, AEKF-SLAM, and ACKF-SLAM algorithms.
(a)
(b)From the simulation results and the theoretical analysis above, we can see that the ACKF-SLAM has higher estimation accuracy with a better numerical performance in the entire motion process than other algorithms. The effectiveness and superiority of the ACKF-SLAM are verified.


## 4.1. Experiment Modeling

In order to verify the effectiveness and feasibility of the new SLAM algorithm proposed in this paper, a large number of simulations are tested with the simulation environment issued by Tim Bailey from University of Sydney. The simulation environment which is a 250 m × 200 m outdoor area includes the movement path and 135 static landmark points. The mobile robot moves along the path from (0, 0) anticlockwise as it is shown in Figure2.Figure 2
Sketch of the simulation environment.The motion model of the mobile robot is shown as follows:(26)xv,k+1=[xvx,k+ΔTvk+1cos⁡(xvφ,k+θk+1)xvy,k+ΔTvk+1cos⁡(xvφ,k+θk+1)xvφ,k+ΔTvk+1sinθk+1WB]+Wk+1,
wherein [xvx,k,xvy,k,xvφ,k]T denotes the pose of the mobile robot at time k; xv,k+1 denotes the pose of the mobile robot at time k-1; ΔT denotes the sampling time; vk+1 denotes the velocity of the mobile robot at time k-1; θk+1 denotes the azimuth angle at time k-1; WB denotes the wheel base between the two axes; Wk+1 denotes the error of the system at time k-1 and W~N(0,Q). The observation model is shown as follows:
(27)zk=[riφi]=[(xi-xvx,k)2+(yi-xvy,k)2arctanyi-xvy,kxi-xvx,k-xvφ,k]+Vk,
wherein (xi,yi) denotes the detected position corresponding to the ith feature; ri denotes the distance between the ith detected feature and the mobile robot; φi denotes the distance between the ith detected feature and the heading of the mobile robot; Vk denotes the measurement error at time k and V~N(0,R).The experiment parameters are set as follows. The initial states of the mobile robotxv,0=[0,0,0]T; the sampling interval ΔT=0.025s; the velocity v=3m/s; the velocity error σv=0.25m/s; the azimuth error σθ=2°; the maximum angular rate is 0.2s; the maximum distance of measurement is 30m; the distance error σr=0.2m; the angular error σφ=1°.


## 4.2. Experimental Results and Analysis

Under the above conditions, fifty Monte Carlo simulations were performed for four SLAM algorithms, including the EKF-SLAM, CKF-SLAM, adaptive EKF-SLAM (AEKF-SLAM), and adaptive CKF-SLAM (ACKF-SLAM). And then the root mean square errors (RMSE) of the estimated results were compared.Figures3 and 4 show the comparisons of the EKF-SLAM and the CKF-SLAM; Figures 5 and 6 show the comparisons of the AEKF-SLAM and the ACKF-SLAM, wherein Figures 4 and 6 are partial enlargements of the rectangle regions in Figures 3 and 5, respectively. System noise Q and observation noise R are shown as follows:
(28)Q=[(0.25)200(3π180)2],R=[(0.1)200(π180)2].Figure 3
Comparisons of the simulation results with EKF-SLAM and CKF-SLAM.Figure 4
Partial enlargement of Figure3.Figure 5
Comparisons of the simulation results with AEKF-SLAM and ACKF-SLAM.Figure 6
Partial enlargement of Figure5.From Figures3 to 6, we obviously know that the estimated error of the ACKF-SLAM is much smaller than that of the CKF-SLAM and EKF-SLAM. It means that the ACKF-SLAM algorithm can provide higher estimated accuracy of the nonlinear SLAM system than the other three solutions.Figures7 and 8 show the RMSEs of the estimated position errors of the ACKF-SLAM, AEKF-SLAM, CKF-SLAM, and EKF-SLAM.Figure 7
Comparisons of the RMSEs of thex-axis estimated position errors.Figure 8
Comparisons of the RMSEs of they-axis estimated position errors.From the results shown in Figures7 and 8, we learn that the RMSE values of the position estimation errors of the ACKF-SLAM algorithm are within 3 m. The precision of AEKF-SLAM is nearly the same as CKF-SLAM, and both of them are lower than ACKF-SLAM algorithm. The RMSE values of the AEKF-SLAM are within 8 m while the corresponding RMSEs of the CKF-SLAM are within 10 m. The EKF-SLAM algorithm has the lowest estimation precision and the RMSEs are within 17 m. So the precision of ACKF-SLAM is higher than the other three algorithms. Comparing between these four algorithms, the estimation precisions of the standard CKF-SLAM and ACKF-SLAM are higher than those of standard EKF-SLAM and AEKF-SLAM because the cubature points are used in the CKF instead of the nonlinear transformation in the EKF. As a result, the errors can be reduced and the filtering precisions can be enhanced availably. We also can see that the estimation errors increase rapidly in the last part of the standard EKF-SLAM and CKF-SLAM. However, the estimation results of the ACKF-SLAM and AEKF-SLAM always keep stable. It is because the system process noise and observation noise are estimated and modified on real time to enhance the filter precision using the adaptive algorithm. These coincide with the theoretical analysis totally.Table1 shows the run time of the four algorithms. The calculated cost of the EKF-SLAM is the least one, the ones of the AEKF-SLAM and the CKF-SLAM are almost equal, and the one of the ACKF-SLAM is the most. That is because of the fact that the EKF only uses the Taylor expansion to linearize the nonlinear system which is extremely simple. But the CKF algorithm approximates the mean and variance by a set of 2N cubature points, so it costs a longer time than the EKF. As the adaptive algorithm is adopted in adaptive CKF-SLAM algorithm, it costs much longer time than the CKF-SLAM algorithm.Table 1
Comparisons of the run time.
SLAM algorithm
Run time (seconds)
Standard EKF-SLAM
196.18
Standard CKF-SLAM
278.52
Adaptive EKF-SLAM
231.74
Adaptive CKF-SLAM
344.39In order to verify the validity of the ACKF-SLAM sufficiently, some different simulations were done with different environment noise. Suppose that the observation noise obeys the mixture Gauss distribution asVk~0.5N(0,R1)+0.5N(0,R2), wherein
(29)R1=[(0.1)20.01π1800.01π180(π180)2],R2=[(0.01)20.3π1800.3π180(3π180)2].Figure9 illustrates the estimated errors of the ACKF-SLAM and the AEKF-SLAM algorithms and Figure 10 is the partial enlargement of the rectangle region in Figure 9. As can be seen from Figures 9 to 11, the mobile robot motion trajectory still can be estimated with a high precision by the ACKF-SLAM even though different environment noise is added.Figure 9
Comparisons of simulation results about the AEKF-SLAM and ACKF-SLAM.Figure 10
Partial enlargement of Figure9.RMSEs of position errors with the AEKF-SLAM and ACKF-SLAM algorithms.
(a)
(b)To verify the effectiveness and superiority of the new adaptive SLAM algorithm, when the system noises are time-varying, another simulation scenario was set in which the process and the observation noises are time-varying. On the basis of [2], the parameters are set as Table 2.Table 2
Simulation parameters.
Calculation step
Q
k
R
k
1
<
t
<
3000
diag
⁡
(
[
(
0.1
)
2
;
(
0.5
π
/
180
)
2
]
)
diag
⁡
(
[
(
0
.
5
)
2
;
(
0
.
2
π
/
180
)
2
]
)
3000
≤
t
<
6000
diag
⁡
(
[
(
0
.
5
)
2
;
(
2
π
/
180
)
2
]
)
diag
⁡
(
[
(
0
.
2
)
2
;
(
π
/
180
)
2
]
)
6000
≤
t
diag
⁡
(
[
(
2
)
2
;
(
4
π
/
180
)
2
]
)
diag
⁡
(
[
1
2
;
(
2
π
/
180
)
2
]
)Figure12 illustrates the estimation errors of the ACKF-SLAM, AEKF-SLAM, and CKF-SLAM algorithms when the system noises are time-varying and Figure 13 is the partial enlargement of the rectangle region in Figure 12. Figure 14 is the RMSEs of the position errors correspondingly. As can be seen from Figures 12 to 14, the estimated errors of the CKF-SLAM are the biggest and the ones of the ACKF-SLAM and the AEKF-SLAM are much smaller. That is because of the fact that when the system noises are time-varying, standard CKF-SLAM cannot accurately estimate them. However, the adaptive CKF-SLAM and adaptive EKF-SLAM can estimate and modify the parameters of the system model and the statistic of the system noises online to decrease the estimated errors and improve the accuracy effectively, owing to the Sage-Husa adaptive estimator. Additionally, comparing between the two adaptive methods, the estimated results of the ACKF-SLAM are much better than that of the AEKF-SLAM.Figure 12
Comparisons of simulation results with time-varying noises.Figure 13
Partial enlargement of Figure12.RMSEs of position errors of the CKF-SLAM, AEKF-SLAM, and ACKF-SLAM algorithms.
(a)
(b)From the simulation results and the theoretical analysis above, we can see that the ACKF-SLAM has higher estimation accuracy with a better numerical performance in the entire motion process than other algorithms. The effectiveness and superiority of the ACKF-SLAM are verified.


## 5. Conclusions and Future Work

In this paper, a novel nonlinear SLAM algorithm based on the CKF and the Sage-Husa adaptive estimator was proposed in order to solve the problem that the noise statistic of the system is unknown or time-varying in real world and to increase the accuracy of the estimation. Firstly, the Sage-Husa adaptive noise estimator was introduced and the superiority of the CKF solution was analyzed theoretically. Secondly, we focused on proposing a new adaptive SLAM based on the CKF method through combining the advantages of the Sage-Husa adaptive estimator and the CKF-SLAM solution, and the novel ACKF-SLAM algorithm proposed here can estimate and correct the statistical character of the noises in real time and decrease the estimated errors. To verify the new SLAM algorithm, numerical simulations in different scenarios were carried out. The results showed that the proposed adaptive SLAM algorithm based on CKF can not only be applied to the systems in which the noise distribution is unknown or time-varying, but also significantly improve the estimation accuracy of the nonlinear SLAM system than traditional SLAM algorithms. The ACKF-SLAM algorithm provides a new method for simultaneous localization and mapping in an unknown environment. However, the calculation burden of the ACKF algorithm is still huge. Reference [27] presented graph-based SLAM algorithms which can reduce the calculation cost significantly using a graph whose nodes correspond to the poses of the robot at different points in time and whose edges represent constraints between the poses. As a result, we plan to work on the adaptive graph-based SLAM algorithm to reduce the computation time at no sacrifice of the accuracy in the future.



---

*Source: 171958-2014-05-07.xml*