Mathematical Problems in Engineering

Volume 2014 (2014), Article ID 171958, 11 pages

http://dx.doi.org/10.1155/2014/171958

Research Article

A SLAM Algorithm Based on Adaptive Cubature Kalman Filter

Fei Yu,¹ Qian Sun,1,2 Chongyang Lv,³ Yueyang Ben,¹ and Yanwei Fu³

¹College of Automation, Harbin Engineering University, Harbin, Heilongjiang 150001, China

²Department of Earth and Space Science and Engineering, York University, Toronto, ON, Canada, M3J 1P3

³College of Science, Harbin Engineering University, Harbin, Heilongjiang 150001, China

Received 2 February 2014; Revised 3 April 2014; Accepted 8 April 2014; Published 7 May 2014

Academic Editor: Dongbing Gu

Copyright © 2014 Fei Yu et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

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

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

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

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:

wherein is the input control of the robot at time which drives from pose at time to pose at time ; is observation to the landmark at time ; is the motion model which is the conditional probability of at time when and 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:

wherein and are the state vector and the measurement vector at time , respectively; and are specific known nonlinear functions; and and are the noise vectors from two independent Gaussian processes with their means being and and their covariance matrices being 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:

wherein . Under the assumption that the posterior density at time 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

(2)evaluate the cubature points

(3)evaluate the propagated cubature points

(4)estimate the predicted state

(5)estimate the predicted error covariance

Measurement-update:

(1)factorize

(2)evaluate the cubature points

(3)evaluate the propagated cubature points

(4)estimate the predicted measurement

(5)estimate the innovation covariance matrix

(6)estimate the cross-covariance matrix

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

(2)Estimate the updated state

(3)Estimate the corresponding error covariance

The CKF-SLAM algorithm uses cubature rule and 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

wherein is the error covariance matrix of the state , is the filter gain matrix, 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 , , , , and is obtained by recurrence of the time-varying noise statistics estimator:

wherein denotes the innovation sequence; 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:

Measurement-update:

With the new measurement vector , the estimation of the state vector and its covariance matrix at time can be obtained by the following equations:

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:

wherein denotes the pose of the mobile robot at time ; denotes the pose of the mobile robot at time ; denotes the sampling time; denotes the velocity of the mobile robot at time ; denotes the azimuth angle at time ; denotes the wheel base between the two axes; denotes the error of the system at time and . The observation model is shown as follows:

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 .

The experiment parameters are set as follows. The initial states of the mobile robot ; the sampling interval ; the velocity ; the velocity error ; the azimuth error ; the maximum angular rate is 0.2; the maximum distance of measurement is 30; the distance error ; the angular error .

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:

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.

SLAM algorithm
Run time (seconds)

Standard EKF-SLAM
196.18

Standard CKF-SLAM
278.52

Adaptive EKF-SLAM
231.74

Adaptive CKF-SLAM
344.39

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 , wherein

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.

Calculation step

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).

References

W. Zhang, M. Zhu, and Z. Chen, “An adaptive SLAM algorithm based on strong tracking UKF,” Robot, vol. 32, no. 2, pp. 190–195, 2010.

H. Wang, G. Fu, J. Li, Z. Yan, and X. Bian, “An adaptive UKF based SLAM method for unmanned underwater vehicle,” Mathematical Problems in Engineering, vol. 2013, Article ID 605981, 12 pages, 2013.

E. Guerra, R. Munguia, Y. Bolea, and A. Grau, “Validation of data association for monocular slam,” Mathematical Problems in Engineering, vol. 2013, Article ID 671376, 11 pages, 2013.

H. Durrant-Whyte and T. Bailey, “Simultaneous localization and mapping: part I,” IEEE Robotics and Automation Magazine, vol. 13, no. 2, pp. 99–108, 2006.

K. P. B. Chandra, D. W. Gu, and I. Postlethwaite, “Cubature kalman filter based localization and mapping,” World Congress, vol. 18, no. 1, pp. 2121–2125, 2011.

A. Nüchter, H. Surmann, K. Lingemann, J. Hertzberg, and S. Thrun, “6D SLAM with an application in autonomous mine mapping,” in Proceedingsof the IEEE International Conference on Robotics and Automation (ICRA '04), vol. 2, pp. 1998–2003, May 2004.

A. Chatterjee and F. Matsuno, “A Geese PSO tuned fuzzy supervisor for EKF based solutions of simultaneous localization and mapping (SLAM) problems in mobile robots,” Expert Systems with Applications, vol. 37, no. 8, pp. 5542–5548, 2010.

S. Huang and G. Dissanayake, “Convergence and consistency analysis for extended Kalman filter based SLAM,” IEEE Transactions on Robotics, vol. 23, no. 5, pp. 1036–1049, 2007.

G. Sun, M. Wang, and L. Wu, “Unexpected results of Extended Fractional Kalman Filter for parameter identification in fractional order chaotic systems,” International Journal of Innovative Computing, Information and Control, vol. 7, no. 9, pp. 5341–5352, 2011.

S. J. Julier and J. K. Uhlmann, “A new extension of the kalman filter to nonlinear systems,” in Proceedings of the International Symposium on Aerospace/Defense, Sensing, Simulation, and Controls, vol. 3, no. 26, p. 3.2, Orlando, Fla, USA, 1997.

X. Yan, C. Zhao, and J. Xiao, “A novel fastslam algorithm based on iterated unscented kalman filter,” in Proceedings of the IEEE International Conference on Robotics and Biomimetics (ROBIO '11), pp. 1906–1911, 2011.

I. Arasaratnam and S. Haykin, “Cubature kalman filters,” IEEE Transactions on Automatic Control, vol. 54, no. 6, pp. 1254–1269, 2009.

I. Arasaratnam, S. Haykin, and T. R. Hurd, “Cubature Kalman filtering for continuous-discrete systems: theory and simulations,” IEEE Transactions on Signal Processing, vol. 58, no. 10, pp. 4977–4993, 2010.

W. Gao, Y. Zhang, and J. Wang, “A strapdown interial navigation system/beidou/doppler velocity log integrated navigation algorithm based on a cubature kalman filter,” Sensors, vol. 14, no. 1, pp. 1511–1527, 2014.

X. Tang, J. Wei, and K. Chen, “Square-root adaptive cubature kalman filter with application to spacecraft attitude estimation,” in Proceedings of the 15th International Conference on Information Fusion, pp. 1406–1412, 2012.

R. K. Mehra, “On the identification of variances and adaptive Kalman filtering,” IEEE Transactions on Automatic Control, vol. 15, no. 2, pp. 175–184, 1970.

F. R. Fitzgerald, “Divergence of the Kalman filter,” IEEE Transactions on Automatic Control, vol. 16, no. 6, pp. 736–747, 1971.

P. Batista, C. Silvestre, and P. J. Oliveira, “Kalman and H infinity optimal filtering for a class of kinematic systems,” in Proceedings of the 17th World Congress, International Federation of Automatic Control (IFAC '08), July 2008.

Z. Wang, B. Shen, and X. Liu, “ filtering with randomly occurring sensor saturations and missing measurements,” Automatica, vol. 48, no. 3, pp. 556–562, 2012.

J.-H. Wang, C.-L. Song, X.-T. Yao, and J.-B. Chen, “Sigma point H-infinity filter for initial alignment in marine strapdown inertial navigation system,” in Proceedings of the 2nd International Conference on Signal Processing Systems (ICSPS '10), vol. 1, pp. V1580–V1584, July 2010.

L. Zhao and X. Wang, “An adaptive UKF with noise statistic estimator,” in Proceedings of the 4th IEEE Conference on Industrial Electronics and Applications (ICIEA '09), pp. 614–618, May 2009.

G. Sun, M. Wang, L. Huang, and L. Shen, “Generating multi-scroll chaotic attractors via switched fractional systems,” Circuits, Systems, and Signal Processing, vol. 30, no. 6, pp. 1183–1195, 2011.

J. Wang, J. Liu, and B.-G. Cai, “Study on information fusion algorithm in embedded integrated navigation system,” in Proceedings of the International Conference on Intelligent Computation Technology and Automation (ICICTA '08), vol. 2, pp. 1007–1010, October 2008.

H. Wang, J. Wang, X. Bian, and G. Fu, “SLAM of AUV based on the combined EKF,” Jiqiren/Robot, vol. 34, no. 1, pp. 56–64, 2012.

S. Kawamura and N. Sakagami, “Planning and control of robot motion based on time-scale transformation,” in Advances in Robot Control, pp. 157–178, Springer, New York, NY, USA, 2006.

S. Mei, F. Liu, and A. Xue, The Semi Tensor Product Methods in the Power System Transient Analysis, Tsinghua University Press, 2010.

G. Grisetti, R. Kummerle, C. Stachniss, and W. Burgard, “A tutorial on graph-based SLAM,” IEEE Intelligent Transportation Systems Magazine, vol. 2, no. 4, pp. 31–43, 2010.