An Adaptive Unscented Kalman Filtering Algorithm for MEMS/GPS Integrated Navigation Systems

MEMS/GPS integrated navigation systemhas beenwidely used for land-vehicle navigation.This systemexhibits large errors because of its nonlinear model and uncertain noise statistic characteristics. Based on the principles of the adaptive Kalman filtering (AKF) and unscented Kalman filtering (AUKF) algorithms, an adaptive unscented Kalman filtering (AUKF) algorithm is proposed. By using noise statistic estimator, the uncertain noise characteristics could be online estimated to adaptively compensate the timevarying noise characteristics. Employing the adaptive filtering principle into UKF, the nonlinearity of system can be restrained. Simulations are conducted for MEMS/GPS integrated navigation system. The results show that the performance of estimation is improved by the AUKF approach compared with both conventional AKF and UKF.


Introduction
Microelectromechanical systems (MEMS) and Global Positioning System (GPS) integrated navigation system have the advantages of small size, light weight, and low cost, but, because of its low accuracy, it can only be applied in low accuracy navigation fields such as unmanned aircrafts and land-vehicles [1,2]. There are three main factors impacting its performance: (a) the inaccuracy of model parameters, (b) the uncertainty of measurement and observation noise statistic properties, and (c) the nonlinearity of model [3,4].
The classical Kalman filter (KF) provides a recursive solution for estimation of linear dynamic systems. The optimality of the KF algorithm is mainly dependent on a priori statistic of the process and measurement noise and the linear system model. However, if the priori information is insufficient or biased, the precision of the estimated states will be degraded, even leading to divergences [5]. In the case of MEMS/GPS applications, the estimation system tends to be nonlinear as well as variational noise properties [6]. Meanwhile, for the vehicle navigation, there are many sudden motion changes. To deal with these problems, the implement of AKF appears to be one of suitable approaches [7].
By utilizing the innovation and residual information, the AKF could adapt the filter stochastic properties online to accommodate itself to changes in vehicle dynamics. Thus, this technique could reduce the reliance of filter on the prior statistical information and obtain the noise statistic parameters of the dynamic system. The essence of AKF is to adapt the filter weights, so as to restrain model errors and improve the accuracy of filters. It is showed that applying AKF to the INS/GPS integrated navigation system could obtain better estimated performance than by using conventional KF, especially less than 20% root mean square error in attitude estimation [8]. However, AKF is unreliable when it is applied into the nonlinear applications.
UKF, which is another extension of Kalman filter, could give reliable estimates even if the nonlinearities of system are quiet severe [9]. The linearization procedure is avoided by introducing the unscented transformation (UT), which is a method to approximate the joint distribution of states and measurements variables. In UT, a number of sigma points are chosen which could maintain the desired mean and covariance of states. Theoretically, the performance of UKF could be close to that of the three-order Taylor series expansion approximation or better than it [10]. However, many kinds of systems such as aircrafts, ships, and spacecrafts would be intensively disturbed by external environment. As a result, the statistical properties for system process and measurement noises cannot be predicted, and UKF cannot solve these problems effectively.
As a combination of AKF and UKF, the adaptive UKF has been developed and applied to nonlinear joint estimation of both time-varying states and parameters [11]. The adaptive principles have been employed to update the means and covariances of the process and measurement noises online. The contributions of model predicted states and measurement information for filtering are balanced. In this paper, a new AUKF algorithm is proposed for MEMS/GPS integrated navigation systems in vehicle applications. A noise estimator for UKF is designed to estimate and update the means and covariances of noises online. Then, the updated means and covariances are propagating through the UKF. The proposed AUKF has the adaptive ability to time-varying noises and the noise estimates are unbiased. The performance of AUKF applied in land navigation is evaluated by simulations and the results show that the integrated system exhibits excellent robustness and navigation performance.

Unscented Kalman Filtering Algorithm for Nonzero Mean Noise
In the integrated navigation field, almost all of the systems are nonlinear. The general nonlinear discrete system model is given as where ∈ is the state vector, ∈ is the measurement vector, and (⋅) ∈ × and ℎ (⋅) ∈ × are the state and measurement matrices of nonlinear system, respectively. and V are the Gaussian white noise which are unrelated. The mean and covariance of and V are given as follows: where and are nonzero constant variables and is a Kronecker delta function.
The initial state 0 is uncorrelated with both the process noise and measurement noise. These initial states exhibit Gaussian normal distributions. The prior mean and covariance matrices of 0 are defined bŷ Assuming that = − and = V − and substituting them into (1) yield where the mean and covariance of and are given as follows: According to system model of (4), the recursive solution of UKF algorithm is noted as follows.
(1) Sigma Points Sampling. In order to guarantee positive semidefinite of state covariance, the modified sigma points sampling solution based on the scaling method is adopted [12].
Choose 2 + 1 sigma points , / −1 as follows: where is the covariance of the state vector , √( + ) is the matrix square root of , and ( √( + ) ) denotes the th row items of √( + ) .
Then, computing the predicted statê/ −1 , the predicted covariance / −1 is as follows: where and are associated weights: Parameter is a scaling parameter, which is defined by Journal of Applied Mathematics 3 The parameters , , and are the positive constants in the sampling method. Then, propagating the sigma points , / −1 , the measurement function ℎ (⋅) + yields Computing the predicted measurement vector̂/ −1 , the covariance of the measurement̃and the cross-covariance of the state and measurement̃̃are as follows: (3) Updating. Then, computing the filter gain , the updated statê/ and covariance / are as follows:

Noise Statistic Estimator
Aiming at the uncertainty of process and measurement noise statistic properties, the measurement information are used to real time estimate and update the means and covariances of noises. Assume that and V are uncorrelated and obey the Gaussian distributions. Based on the maximum a posterior (MAP) principle, a noise statistic estimator is derived.
The noise parameters , and the noise matrices , are unknown and need to be estimated according to the updated measurements. The MAP estimates of , , , and the state are denoted aŝ,̂,̂,̂, and̂/ , respectively. The conditional distribution of interest based on the measurements is expressed as Because [ ] is disrelated to other parameters except the estimate state, the problem in calculating (16) changes to calculate the joint conditional probability distribution: where [ , , , ] can be treated as a constant which is provided by the prior statistic information.
where is the dimension of system state, 1 = 1/(2 ) ( +1)/2 is a constant, and | | is the determinant of and ‖ ‖ 2 = . Moreover, assuming that the measurements 1 , 2 , . . . , are known and unrelated to each other, the distribution of is Gaussian, which can be expressed as where denotes the dimension of measurements and 2 = 1/(2 ) /2 is a constant.
Substituting (18) and (19) into (17) yields where Logarithm on both sides of (20) yields By the logarithmic nature, and ln share the same extreme points. The partial derivative of can be calculated by the following equations: Then, the noise statistic estimator can be derived, which is defined bŷ= In (24) to (27), the smoothed estimateŝ− 1/ and̂/ can be replaced by the filtered estimatê/ or the predicted statê / −1 as approximating solutions.

Noise Statistic Estimator for UKF
From the consideration for the nonlinear purposes, the noise statistic estimator derived above should be modified. In the linear applications, the term of −1 ( −1 ) can be obtained by propagating each estimatê− 1 through system model and ℎ ( ) by propagatinĝthrough measurement equation. However, for the nonlinear field, the scaled sigma points are inserted instead of the estimate. The predicted term for UKF could be expressed as a combination of all sigma points: where −1 ( −1 ) is approximated by UT with a precision close to that using three-order Taylor series expansion method [14]. Similarly, ℎ ( ) can be calculated by Submitting (28) into (24) to (27) yields the noise statistic estimator for UKF: The unbiased properties of the noise estimates for UKF are proved in the appendix.

Recursive Equations of Adaptive Unscented Kalman Filtering Algorithm
Based on the UKF and its noise statistic estimator, the prediction and update steps of AUKF algorithm are as follows.
With real-time measurement noise parameters, compute the predicted measurement̂/ −1 and the covariancẽaŝ Estimate the cross-covariancẽ̃as̃= Then compute the filter gain , the estimated state vector / , and its covariance / :

MEMS/GPS Integrated Navigation for Land-Vehicle Using AUKF
Because of the highly nonlinear characteristic of MEMS/GPS, the conventional AKF based on small angle approximations is limited. Meanwhile, due to the time-varying noise stochastic properties for land-vehicle, the standard UKF in Section 1 cannot be directly applied to integrated navigation. On the other side, the modified AUKF based on a statistic estimator in Section 4 appears appropriate for the MEMS/GPS integrated navigation. Simulations are conducted to compare the performances of AKF, UKF, and AUKF. In the simulation, the parameters of sensor errors are shown in Table 1. The initial position of vehicle is east longitude 126 ∘ and north latitude 45 ∘ . Figure 1 shows the trajectory of land-vehicle motion. The solid line in this figure illustrates the real simulated trajectory.
The architecture of MEMS/GPS integrated navigation with AUKF is shown in Figure 2.
As shown in Figures 3, 4, and 5, with the comparisons with AKF and UKF, the AUKF with noise estimator could obviously improve the accuracy of the velocity solutions. In addition, as shown in Figure 3, because the noise statistic estimators are designed in AKF and AUKF, the position errors of the two filters exhibit similar characteristics at most of the time in this simulation, and the performance of AUKF is slightly better. However, which is also seen in Figure 3  are so notable vibrations for UKF in which stable estimate errors of positions could not be provided, which are mainly caused by the quickly changed system noises. Figure 4 shows that the AUKF always has smaller velocity errors than AKF and UKF. And Figure 5 shows that at most of the time the AUKF scheme has a better performance on attitudes errors. The AKF and UKF solutions have large vibration errors in both Figures 4 and 5. That is because the velocity errors are related to the attitude errors, especially to the pitch and roll errors of AKF and UKF. Because of the strong nonlinear properties of system, it is difficult that AKF cannot be effectively operated for navigation. Meanwhile, for UKF solutions, the curves of speed errors are extremely similar to those of horizontal attitude errors.
The simulation results indicate that if the AUKF scheme is considered the filtering solution, there are only small variations impacting on the performance of MEMS/GPS integrated navigation system, and this system has an excellent robustness. However, long processing time would cause slight divergence of the attitude errors, sequentially the velocity and position errors.

Conclusions
This study has developed an AUKF approach to improve the navigation performance of MEMS/GPS integrated system for land-vehicle applications. By treating this problem within conventional UKF framework, the noise estimator is adopted and could effectively estimate the process and measurement noise characteristics online. The results indicate that the proposed AUKF algorithm could efficiently improve the navigation performance of land-vehicle integrated navigation system. By comparing with AKF and UKF methods, the AUKF solution has a more stable and superior performance.

Appendix
The process noise and measurement noise statistic properties are computed by estimator in (30) to (33). Their unbiased properties are proved as follows.
According to (8), the predicted state at time step is given aŝ/ Thus, the estimate of the process noise noted by (24) is unbiased.
Similarly, the estimate of the measurement noise is [̂] = . (A.7) The unbiased properties for the estimates of noises are proved.