A SLAM Algorithm Based on Adaptive Cubature Kalman Filter

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.

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 𝐻_∞ filter algorithm and the adaptive filter algorithm which have been focused on in recent years. Although the 𝐻_∞ 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 Section 2. 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 vector 𝑋_𝑣 and the covariance matrix 𝑃_𝑣𝑣, defined as 𝑋_𝑣=𝑥_𝑣𝑦_𝑣𝜑_𝑣^𝑇,𝑃_𝑣𝑣=𝜎²_{𝑥𝑣𝑥𝑣}𝜎²_{𝑥𝑣𝑦𝑣}𝜎²_{𝑥𝑣𝜑𝑣}𝜎²_{𝑥𝑣𝑦𝑣}𝜎²_{𝑦𝑣𝑦𝑣}𝜎²_{𝑦𝑣𝜑𝑣}𝜎²_{𝑥𝑣𝜑𝑣}𝜎²_{𝑦𝑣𝜑𝑣}𝜎²_{𝜑𝑣𝜑𝑣}. (1)

Without loss of generality, let us assume that there are a set of 𝑛 2D static point features observed in the map; the position estimations of these features are given by their estimated vector 𝑋_𝑚 and the covariance matrix 𝑃_𝑚𝑚 as 𝑋_𝑚=𝑥₁𝑦₁⋯𝑥_𝑛𝑦_𝑛^𝑇,𝑃_𝑚𝑚=𝜎²_𝑥1𝑥1𝜎²_𝑥1𝑦1⋯𝜎²_{𝑥1𝑥𝑛}𝜎²_{𝑥1𝑦𝑛}𝜎²_𝑥1𝑦1𝜎²_𝑦1𝑦1⋯𝜎²_{𝑦1𝑥𝑛}𝜎²_{𝑦1𝑦𝑛}𝜎⋮⋮⋱⋮⋮_{2𝑥1𝑥𝑛}𝜎²_{𝑦1𝑥𝑛}⋯𝜎²_{𝑥𝑛𝑥𝑛}𝜎²_{𝑥𝑛𝑦𝑛}𝜎²_{𝑥1𝑦𝑛}𝜎²_{𝑦1𝑦𝑛}⋯𝜎²_{𝑥𝑛𝑦𝑛}𝜎²_{𝑦𝑛𝑦𝑛}. (2)

In the SLAM, the total state vector 𝑋 is composed of the robots states 𝑋_𝑣 and the landmarks states 𝑋_𝑚. Hence the estimations of the total state vector 𝑋 and the corresponding total error covariance matrix 𝑃 are given as 𝑋=𝑋^𝑇_𝑣𝑋^𝑇_𝑚^𝑇,𝑃𝑃=^𝑣𝑣𝑃_𝑣𝑚𝑃^𝑇_𝑣𝑚𝑃_𝑚𝑚, (3) wherein 𝑃_𝑣𝑚 denotes the robot-map correlation.

The key of the SLAM is to determine the posterior probability density of the state vector 𝑋 which is 𝑝(𝑋_𝑣,𝑘,𝑋_𝑚∣𝑢_𝑘,𝑧_𝑘) and the implication is to obtain the joint probability density of the robot pose 𝑋_𝑣,𝑘 and the map 𝑋_𝑚 when the control input 𝑢_𝑘 and observation 𝑧_𝑘 are known. We can obtain the following equation by utilizing the Bayes formula: 𝑝𝑋_𝑣,𝑘,𝑋_𝑚∣𝑢_𝑘,𝑧_𝑘=𝑝𝑧_𝑘∣𝑋_𝑣,𝑘,𝑋_𝑚×𝑝𝑋_𝑣,𝑘∣𝑋_{𝑣,𝑘−1},𝑢_𝑘×𝑝𝑋_{𝑣,𝑘−1},𝑋_𝑚∣𝑢_𝑘−1,𝑧_𝑘−1𝑑𝑋_{𝑣,𝑘−1}, (4) wherein 𝑢_𝑘 is the input control of the robot at time 𝑘 which drives from pose 𝑋_{𝑣,𝑘−1} at time 𝑘−1 to pose 𝑋_𝑣,𝑘 at time 𝑘; 𝑧_𝑘 is observation to the landmark at time 𝑘; 𝑝(𝑋_𝑣,𝑘∣𝑋_{𝑣,𝑘−1},𝑢_𝑘) is the motion model which is the conditional probability of 𝑋_𝑣,𝑘 at time 𝑘 when 𝑢_𝑘 and 𝑋_{𝑣,𝑘−1} are known; 𝑝(𝑧_𝑘∣𝑋_𝑣,𝑘,𝑋_𝑚) is the observation model which is the conditional probability of 𝑧_𝑘 at time 𝑘 when 𝑋_𝑣,𝑘 and the landmark collection matrix 𝑋_𝑚 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: 𝑥_𝑘=𝑓𝑥_𝑘−1+𝑊_𝑘−1,𝑧_𝑘=ℎ𝑥_𝑘+𝜂_𝑘, (5) wherein 𝑥_𝑘 and 𝑧_𝑘 are the state vector and the measurement vector at time 𝑘, respectively; 𝑓(⋅) and ℎ(⋅) are specific known nonlinear functions; and 𝑊_𝑘−1 and 𝜂_𝑘 are the noise vectors from two independent Gaussian processes with their means being 𝑞_𝑘−1 and 𝑟_𝑘 and their covariance matrices being 𝑄_𝑘−1 and 𝑅_𝑘, 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 2𝑁 (𝑁 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 2𝑁 cubature points are given by [𝜉_𝑖,𝜔_𝑖], where 𝜉_𝑖 is the 𝑖th cubature point and 𝜔_𝑖 is the corresponding weight: 𝜉_𝑖=√𝑁[1]_𝑖,𝜔_𝑖=1,2𝑁 (6) wherein 𝑖=1,2,…,2𝑁. Under the assumption that the posterior density at time 𝑘−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 𝑃_{𝑘−1∣𝑘−1}=𝑆_{𝑘−1∣𝑘−1}𝑆^𝑇_{𝑘−1∣𝑘−1}, (7)(2)evaluate the cubature points (𝑖=1,2,…,2𝑁)𝑋_{𝑖,𝑘−1∣𝑘−1}=𝑆_{𝑘−1∣𝑘−1}𝜉_𝑖+𝑥_𝑘−1, (8)(3)evaluate the propagated cubature points 𝑋^∗_{𝑖,𝑘∣𝑘−1}=𝑓𝑋_{𝑖,𝑘−1∣𝑘−1}+𝑞_𝑘−1, (9)(4)estimate the predicted state 𝑥_{𝑘∣𝑘−1}=12𝑁^2𝑁_𝑖=1𝑋^∗_{𝑖,𝑘∣𝑘−1}, (10)(5)estimate the predicted error covariance 𝑃_{𝑘∣𝑘−1}=12𝑁^2𝑁_𝑖=1𝑋^∗_{𝑖,𝑘∣𝑘−1}𝑋^∗𝑇_{𝑖,𝑘∣𝑘−1}−𝑥_{𝑘∣𝑘−1}𝑥^𝑇_{𝑘∣𝑘−1}+𝑄_𝑘−1. (11)

Measurement-update:(1)factorize 𝑃_{𝑘∣𝑘−1}=𝑆_{𝑘∣𝑘−1}𝑆^𝑇_{𝑘∣𝑘−1}, (12)(2)evaluate the cubature points 𝑋_{𝑖,𝑘∣𝑘−1}=𝑆_{𝑘∣𝑘−1}𝜉_𝑖+𝑥_{𝑘∣𝑘−1}, (13)(3)evaluate the propagated cubature points 𝑌_{𝑖,𝑘∣𝑘−1}=ℎ𝑋_{𝑖,𝑘∣𝑘−1}+𝑟_𝑘, (14)(4)estimate the predicted measurement 𝑦_{𝑘∣𝑘−1}=12𝑁^2𝑁_𝑖=1𝑌_{𝑖,𝑘∣𝑘−1}, (15)(5)estimate the innovation covariance matrix 𝑃^𝑧𝑧_{𝑘∣𝑘−1}=12𝑁^2𝑁_𝑖=1𝑌_{𝑖,𝑘∣𝑘−1}𝑌^𝑇_{𝑖,𝑘∣𝑘−1}−𝑦_{𝑘∣𝑘−1}𝑦^𝑇_{𝑘∣𝑘−1}+𝑅_𝑘, (16)(6)estimate the cross-covariance matrix 𝑃^𝑥𝑧_{𝑘∣𝑘−1}=12𝑁^2𝑁_𝑖=1𝑋_{𝑖,𝑘∣𝑘−1}𝑌^𝑇_{𝑖,𝑘∣𝑘−1}−𝑥_{𝑘∣𝑘−1}𝑦^𝑇_{𝑘∣𝑘−1}. (17)

With the new measurement vector 𝑧_𝑘, the estimation of the state vector 𝑥_𝑘∣𝑘 and its covariance matrix 𝑃_𝑘∣𝑘 at time 𝑘 can be obtained as follows.(1)Estimate the Kalman gain 𝐾_𝑘=𝑃^𝑥𝑧_{𝑘∣𝑘−1}𝑃^𝑧𝑧_{𝑘∣𝑘−1}⁻¹. (18)(2)Estimate the updated state 𝑥_𝑘=𝑥_{𝑘∣𝑘−1}+𝐾_𝑘𝑧_𝑘−𝑧_{𝑘∣𝑘−1}. (19)(3)Estimate the corresponding error covariance 𝑃_𝑘∣𝑘=𝑃_{𝑘∣𝑘−1}−𝐾_𝑘𝑃^𝑧𝑧_{𝑘∣𝑘−1}𝐾^𝑇_𝑘. (20)

The CKF-SLAM algorithm uses cubature rule and 2𝑁 cubature point sets [𝜉_𝑖,𝜔_𝑖] 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 𝑥_{𝑘∣𝑘−1}=𝑓𝑥_𝑘−1+𝑞_𝑘−1,𝑃_{𝑘∣𝑘−1}=𝐹_𝑘−1𝑃_𝑘−1𝐹^𝑇_𝑘−1+𝑄_𝑘−1,𝜉_𝑘=𝑧_𝑘−ℎ𝑥_{𝑘∣𝑘−1}−𝑟_𝑘̂,𝑆_𝑘=𝐻_𝑘𝑃_{𝑘∣𝑘−1}𝐻^𝑇_𝑘+𝑅_𝑘,𝐾_𝑘=𝑃_{𝑘∣𝑘−1}𝐻^𝑇_𝑘𝑆_𝑘⁻¹,𝑥_𝑘=𝑥_{𝑘∣𝑘−1}+𝐾_𝑘𝜉_𝑘,𝑃_𝑘=𝐼−𝐾_𝑘𝐻_𝑘𝑃_{𝑘∣𝑘−1}, (21) wherein 𝑃_𝑘 is the error covariance matrix of the state 𝑥_𝑘, 𝐾_𝑘 is the filter gain matrix, 𝐹_𝑘−1 and 𝐻_𝑘 are the nonlinear states function 𝑓(⋅) and observation function ℎ(⋅) with respect to states, respectively, 𝜉_𝑘 is the difference between the measurement and the prediction, and 𝑆_𝑘 covariance matrix of the 𝜉_𝑘, 𝑞_𝑘−1, 𝑟_𝑘̂, 𝑄_𝑘−1, and 𝑅_𝑘 is obtained by recurrence of the time-varying noise statistics estimator: 𝑞_𝑘=1−𝑑_𝑘−1𝑞_𝑘−1+𝑑_𝑘−1𝑥_𝑘−𝑓𝑥_𝑘−1,𝑄_𝑘=1−𝑑_𝑘−1𝑄_𝑘−1+𝑑_𝑘−1𝐾_𝑘𝜉_𝑘𝜉^𝑇_𝑘𝐾^𝑇_𝑘+𝑃_𝑘−𝐹_𝑘−1𝑃_𝑘−1𝐹^𝑇_𝑘−1,𝑟_𝑘̂=1−𝑑_𝑘−1𝑟̂_𝑘−1+𝑑_𝑘−1𝑧_𝑘−ℎ𝑋_{𝑘∣𝑘−1},𝑅_𝑘=1−𝑑_𝑘−1𝑅_𝑘−1+𝑑_𝑘−1𝜉_𝑘𝜉^𝑇_𝑘−𝐻_𝑘𝑃_{𝑘∣𝑘−1}𝐻^𝑇_𝑘,𝑑_𝑘−1=(1−𝑏)1−𝑏^𝑘, (22) wherein 𝜉_𝑘+1 denotes the innovation sequence; 𝑏(0<𝑏<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 Section 2.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: 𝑃_{𝑘−1∣𝑘−1}=𝑆_{𝑘−1∣𝑘−1}𝑆^𝑇_{𝑘−1∣𝑘−1},𝑋_{𝑖,𝑘−1∣𝑘−1}=𝑆_{𝑘−1∣𝑘−1}𝜉_𝑖+𝑥_𝑘−1,𝑋^∗_{𝑖,𝑘∣𝑘−1}=𝑓𝑋_{𝑖,𝑘−1∣𝑘−1}+𝑞_𝑘−1,𝑥_{𝑘∣𝑘−1}=12𝑁^2𝑁_𝑖=1𝑋^∗_{𝑖,𝑘∣𝑘−1},𝑃_{𝑘∣𝑘−1}=12𝑁^2𝑁_𝑖=1𝑋^∗_{𝑖,𝑘∣𝑘−1}𝑋^∗𝑇_{𝑖,𝑘∣𝑘−1}−𝑥_{𝑘∣𝑘−1}𝑥^𝑇_{𝑘∣𝑘−1}+𝑄_𝑘−1. (23)

Measurement-update: 𝑃_{𝑘∣𝑘−1}=𝑆_{𝑘∣𝑘−1}𝑆^𝑇_{𝑘∣𝑘−1},𝑋_{𝑖,𝑘∣𝑘−1}=𝑆_{𝑘∣𝑘−1}𝜉_𝑖+𝑥_{𝑘∣𝑘−1},𝑌_{𝑖,𝑘∣𝑘−1}=ℎ𝑋_{𝑖,𝑘∣𝑘−1}+𝑟_𝑘̂,𝑧_{𝑘∣𝑘−1}=12𝑁^2𝑁_𝑖=1𝑌_{𝑖,𝑘∣𝑘−1},𝑃^𝑧𝑧_{𝑘∣𝑘−1}=12𝑁^2𝑁_𝑖=1𝑌_{𝑖,𝑘∣𝑘−1}𝑌^𝑇_{|𝑖,𝑘𝑘−1}−𝑧_{𝑘∣𝑘−1}𝑧^𝑇_{𝑘∣𝑘−1}+𝑅_𝑘,𝑃^𝑥𝑧_{𝑘∣𝑘−1}=12𝑁^2𝑁_𝑖=1𝑋_{𝑖,𝑘∣𝑘−1}𝑌^𝑇_{𝑖,𝑘∣𝑘−1}−𝑥_{𝑘∣𝑘−1}𝑧^𝑇_{𝑘∣𝑘−1}. (24)

With the new measurement vector 𝑧_𝑘, the estimation of the state vector 𝑥_𝑘∣𝑘 and its covariance matrix 𝑃_𝑘∣𝑘 at time 𝑘 can be obtained by the following equations: 𝐾_𝑘=𝑃^𝑥𝑧_{𝑘∣𝑘−1}𝑃^𝑧𝑧_{𝑘∣𝑘−1}⁻¹,𝑥_𝑘=𝑥_{𝑘∣𝑘−1}+𝐾_𝑘𝜉_𝑘,𝜉_𝑘=𝑧_𝑘−𝑧_{𝑘∣𝑘−1},𝑃_𝑘∣𝑘=𝑃_{𝑘∣𝑘−1}−𝐾_𝑘𝑃^𝑧𝑧_{𝑘∣𝑘−1}𝐾^𝑇_𝑘,𝑞_𝑘=1−𝑑_𝑘−1𝑞_𝑘−1+𝑑_𝑘−1𝑋_𝑘−𝑥_{𝑘∣𝑘−1},𝑄_𝑘=1−𝑑_𝑘−1𝑄_𝑘−1+𝑑_𝑘−1𝐾_𝑘𝜉_𝑘𝜉^𝑇_𝑘𝐾^𝑇_𝑘+𝑃_𝑘−𝑃_{𝑘∣𝑘−1},𝑟_𝑘̂=1−𝑑_𝑘−1𝑟̂_𝑘−1+𝑑_𝑘−1𝑧_𝑘−𝑧_{𝑘∣𝑘−1},𝑅_𝑘=1−𝑑_𝑘−1𝑅_𝑘−1+𝑑_𝑘−1𝜉_𝑘𝜉^𝑇_𝑘−𝑃^𝑧𝑧_{𝑘∣𝑘−1},𝑑_𝑘−1=(1−𝑏)1−𝑏^𝑘. (25)

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 Figure 2.

Figure 2 

Sketch of the simulation environment.

The motion model of the mobile robot is shown as follows: 𝑥_𝑣,𝑘+1=𝑥_{𝑣𝑥,𝑘}+Δ𝑇𝑣_𝑘+1cos𝑥_{𝑣𝜑,𝑘}+𝜃_𝑘+1𝑥_{𝑣𝑦,𝑘}+Δ𝑇𝑣_𝑘+1cos𝑥_{𝑣𝜑,𝑘}+𝜃_𝑘+1𝑥_{𝑣𝜑,𝑘}+Δ𝑇𝑣_𝑘+1sin𝜃_𝑘+1WB+𝑊_𝑘+1, (26) wherein [𝑥_{𝑣𝑥,𝑘},𝑥_{𝑣𝑦,𝑘},𝑥_{𝑣𝜑,𝑘}]^𝑇 denotes the pose of the mobile robot at time 𝑘; 𝑥_𝑣,𝑘+1 denotes the pose of the mobile robot at time 𝑘−1; Δ𝑇 denotes the sampling time; 𝑣_𝑘+1 denotes the velocity of the mobile robot at time 𝑘−1; 𝜃_𝑘+1 denotes the azimuth angle at time 𝑘−1; WB denotes the wheel base between the two axes; 𝑊_𝑘+1 denotes the error of the system at time 𝑘−1 and 𝑊∼𝑁(0,𝑄). The observation model is shown as follows: 𝑧_𝑘=𝑟_𝑖𝜑_𝑖=𝑥_𝑖−𝑥_{𝑣𝑥,𝑘}²+𝑦_𝑖−𝑥_{𝑣𝑦,𝑘}²𝑦arctan^𝑖−𝑥_{𝑣𝑦,𝑘}𝑥_𝑖−𝑥_{𝑣𝑥,𝑘}−𝑥_{𝑣𝜑,𝑘}+𝑉_𝑘, (27) wherein (𝑥_𝑖,𝑦_𝑖) denotes the detected position corresponding to the 𝑖th feature; 𝑟_𝑖 denotes the distance between the 𝑖th detected feature and the mobile robot; 𝜑_𝑖 denotes the distance between the 𝑖th detected feature and the heading of the mobile robot; 𝑉_𝑘 denotes the measurement error at time 𝑘 and 𝑉∼𝑁(0,𝑅).

The experiment parameters are set as follows. The initial states of the mobile robot 𝑥_𝑣,0=[0,0,0]^𝑇; the sampling interval Δ𝑇=0.025s; the velocity 𝑣=3m/s; the velocity error 𝜎_𝑣=0.25m/s; the azimuth error 𝜎_𝜃=2^∘; the maximum angular rate is 0.2s; the maximum distance of measurement is 30m; the distance error 𝜎_𝑟=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.

Figures 3 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 𝑄 and observation noise 𝑅 are shown as follows: ()𝑄=0.25²003𝜋180²(),𝑅=0.1²00𝜋180². (28)

Figure 3 

Comparisons of the simulation results with EKF-SLAM and CKF-SLAM.

Figure 4 

Partial enlargement of Figure 3.

Figure 5 

Comparisons of the simulation results with AEKF-SLAM and ACKF-SLAM.

Figure 6 

Partial enlargement of Figure 5.

From Figures 3 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.

Figures 7 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 the 𝑥-axis estimated position errors.

Figure 8 

Comparisons of the RMSEs of the 𝑦-axis estimated position errors.

From the results shown in Figures 7 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.

Table 1 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 2𝑁 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.

In 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 as 𝑉_𝑘∼0.5𝑁(0,𝑅₁)+0.5𝑁(0,𝑅₂), wherein 𝑅₁=(0.1)²0.01𝜋1800.01𝜋𝜋180180²,𝑅₂=(0.01)²0.3𝜋1800.3𝜋1803𝜋180². (29)

Figure 9 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 Figure 9.

(a)

(b)

Figure 11 

RMSEs of position errors with the AEKF-SLAM and ACKF-SLAM algorithms.

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.

Figure 12 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 Figure 12.

(a)

(b)

Figure 14 

RMSEs of position errors of the CKF-SLAM, AEKF-SLAM, and ACKF-SLAM algorithms.

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.

Conflict of Interests

The authors declare that there is no conflict of interests regarding the publication of this paper.

Acknowledgments

This work was supported by the Fundamental Research Funds for the Central Universities (HEUCFL1411002) and the National Natural Science Foundation of China (51379047).