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

Authors: Jianhua Cheng; Daidai Chen; Rene Jr. Landry; Lin Zhao; Dongxue Guan
Journal: Journal of Applied Mathematics
 (2014)

---

## Abstract

MEMS/GPS integrated navigation system has been widely used for land-vehicle navigation. This system exhibits 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 time-varying 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.


---

## Body

## 1. 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.


## 2. 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(1)xk=fk-1(xk-1)+wk-1,zk=hk(xk)+vk,
where xk∈Rn is the state vector, zk∈Rm is the measurement vector, and fk(·)∈Rn×n and hk(·)∈Rm×n are the state and measurement matrices of nonlinear system, respectively. wk and vk are the Gaussian white noise which are unrelated. The mean and covariance of wk and vk are given as follows:
(2)E[wk]=q,cov⁡[wkwjT]=Qδkj;E[vk]=r,cov⁡[vkvjT]=Rδkj,
where q and r are nonzero constant variables and δkj is a Kronecker delta function.The initial statex0 is uncorrelated with both the process noise and measurement noise. These initial states exhibit Gaussian normal distributions. The prior mean and covariance matrices of x0 are defined by
(3)x^0=E(x0),P0=cov⁡(x0)=E(x0-x^0)(x0-x^0)T.Assuming thatμk=wk-q and ηk=vk-r and substituting them into (1) yield
(4)xk=fk-1(xk-1)+q+μk-1,zk=hk(xk)+r+ηk,
where the mean and covariance of μk and ηk are given as follows:
(5)E[μk]=0,cov⁡[μkμjT]=Qδkj;E[ηk]=0,cov⁡[ηkηjT]=Rδkj.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].Choose2n+1 sigma points ξi,k/k-1 as follows:
(6)ξ0,k/k-1=x-,ξi,k/k-1=x-+(α(n+λ)P)i,i=1,2,…,n,ξi,k/k-1=x--(α(n+λ)P)i-L,i=n+1,n+2,…,2n,
where P is the covariance of the state vector x, α(n+λ)P is the matrix square root of nP, and (α(n+λ)P)i denotes the ith row items of α(n+λ)P.(2) Prediction. Propagating these sigma points ξi,k/k-1 through nonlinear state function fk(·)+q, we obtain [13]
(7)γi,k/k-1=fk-1(ξi,k/k-1)+q,i=0,1,…,2n.Then, computing the predicted statex^k/k-1, the predicted covariance Pk/k-1 is as follows:
(8)x^k/k-1=∑i=0LWimγi,k/k-1=∑i=0LWimfk-1(ξi,k/k-1)+q,(9)Pk/k-1=∑i=0LWic(γi,k/k-1-x^k/k-1)(γi,k/k-1-x^k/k-1)T+Q,
where Wim and Wic are associated weights:
(10)W0m=λ(n+λ),W0c=λ(n+λ)+(1-α2+β),Wim=12(n+λ),i=1,2,…,2n,Wic=12(n+λ),i=1,2,…,2n.Parameterλ is a scaling parameter, which is defined by
(11)λ=α2(n+κ)-n.
The parameters α, β, and κ are the positive constants in the sampling method.Then, propagating the sigma pointsξi,k/k-1, the measurement function hk(·)+r yields
(12)xi,k/k-1=hk(ξi,k/k-1)+r,i=0,1,…,2n.Computing the predicted measurement vectorz^k/k-1, the covariance of the measurement Pz~k and the cross-covariance of the state and measurement Px~kz~k are as follows:
(13)z^k/k-1=∑i=0LWimxi,k/k-1=∑i=0LWimhk(ξi,k/k-1)+r,(14)Pz~k=∑i=0LWic(xi,k/k-1-z^k/k-1)(xi,k/k-1-z^k/k-1)T+R,Px~kz~k=∑i=0LWic(γi,k/k-1-x^k/k-1)(xi,k/k-1-z^k/k-1)T.(3) Updating. Then, computing the filter gain Kk, the updated state x^k/k and covariance Pk/k are as follows:
(15)Kk=Px~kz~k(Pz~k)-1,x^k/k=x^k/k-1+Kk(zk-z^k/k-1),Pk/k=Pk/k-1-KkPz~k(Kk)T.


## 3. 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 thatwk and vk are uncorrelated and obey the Gaussian distributions. Based on the maximum a posterior (MAP) principle, a noise statistic estimator is derived.The noise parametersq, r and the noise matrices Q, R are unknown and need to be estimated according to the updated measurements. The MAP estimates of q, Q, r, R and the state xk are denoted as q^, Q^, r^, R^, and x^j/k, respectively. The conditional distribution of interest based on the measurements is expressed as
(16)J=p[Xk,q,Q,r,R∣Zk].Becausep[Zk] is disrelated to other parameters except the estimate state, the problem in calculating (16) changes to calculate the joint conditional probability distribution:
(17)J=p[Xk,q,Q,r,R,Zk]=p[Xk∣q,Q,r,R]p[Zk∣Xk,q,Q,r,R]p[q,Q,r,R],
where p[q,Q,r,R] can be treated as a constant which is provided by the prior statistic information.The original problem has changed to calculate the conditional probability distributionsp[Xk∣q,Q,r,R] and p[Zk∣Xk,q,Q,r,R].According to the Gaussian distributions ofμk, the conditional probability distribution p[Xk∣q,Q,r,R] could be expressed as
(18)p[Xk∣q,Q,r,R]=p[x0]∏j=1kp[xj∣xj-1,q,Q]=1(2π)n/2|P0|1/2exp⁡⁡{-12∥x0-x^0∥P0-12}=×{∏j=1k1(2π)n/2|Q|1/2=∏j=1k1(1)n/2|Q|./2×exp⁡⁡{-12∥xj-fj-1(xj-1)-q∥Q-12}}=C1|P0|-1/2|Q|-k/2=×exp⁡⁡{-12∥x0-x^0∥P0-12∑j=1k∥xj-fj-1(xj-1)-q∥Q-12=ex×pp+∑j=1k∥xj-fj-1(xj-1)-q∥Q-12},
where n is the dimension of system state, C1=1/(2π)n(k+1)/2 is a constant, and |A| is the determinant of A and ∥u∥A2=uTAu.Moreover, assuming that the measurementsz1,z2,…,zk are known and unrelated to each other, the distribution of ηk is Gaussian, which can be expressed as
(19)p[Zk∣Xk,q,Q,r,R]=∏j=1kp[zj∣xj,r,R]=∏j=1k1(2π)m/2|R|1/2exp⁡⁡{-12∥zj-hj(xj)-r∥R-12}=C2|R|-k/2exp⁡⁡{-12∑j=1k∥zj-hj(xj)-r∥R-12},
where m denotes the dimension of measurements and C2=1/(2π)mk/2 is a constant.Substituting (18) and (19) into (17) yields
(20)J=C1C2|P0|-1/2|Q|-k/2|R|-k/2p[q,Q,r,R]=C|Q|-k/2|R|-k/2exp⁡⁡{-12[∑j=1k∥xj-fj-1(xj-1)-q∥Q-12=C|Q|-k/200000000|R|-k/2+∑j=1k∥zj-hj(xj)-r∥R-12]},
where
(21)C=exp⁡⁡{-12∥x0-x^0∥P0-12}C1C2|P0|-1/2p[q,Q,r,R].Logarithm on both sides of (20) yields
(22)ln⁡⁡J=-k2ln⁡⁡|Q|-k2ln⁡⁡|R|-12∑j=1k∥xj-fj-1(xj-1)-q∥Q-12-12∑j=1k∥zj-hj(xj)-r∥R-12+ln⁡⁡C.By the logarithmic nature,J and ln⁡⁡J share the same extreme points. The partial derivative of J can be calculated by the following equations:
(23)∂ln⁡⁡J∂q|q=q^kxj-1=x^j-1/k,xj=x^j/k=0,∂ln⁡⁡J∂Q|Q=Q^kxj-1=x^j-1/k,xj=x^j/k=0,∂ln⁡⁡J∂r|r=r^kxj=x^j/k=0,∂ln⁡⁡J∂R|R=R^kxj=x^j/k=0.Then, the noise statistic estimator can be derived, which is defined by(24)q^k=1k∑j=1k[x^j/k-fj-1(x^j-1/k)],(25)Q^k=1k∑j=1k{[x^j/k-fj-1(x^j-1/k)-q][(KL^j/k)]K[(KL^j/k)]K×[x^j/k-fj-1(x^j-1/k)-q]T},(26)r^k=1k∑j=1k[zj-hj(x^j/k)],(27)R^k=1k∑j=1k{[zj-hj(x^j/k)-r][zj-hj(x^j/k)-r]T}.In (24) to (27), the smoothed estimates x^j-1/k and x^j/k can be replaced by the filtered estimate x^j/j or the predicted state x^j/j-1 as approximating solutions.


## 4. 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 offj-1(xj-1) can be obtained by propagating each estimate x^j-1 through system model and hj(xj) by propagating x^j 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:
(28)fj-1(xj-1)|xj-1←x^j-1/j-1=∑i=0LWimfj-1(ξi,j-1/j-1),
where fj-1(xj-1) is approximated by UT with a precision close to that using three-order Taylor series expansion method [14].Similarly,hj(xj) can be calculated by
(29)hj(xj)|xj←x^j/j-1=∑i=0LWimhj(ξi,j/j-1).Submitting (28) into (24) to (27) yields the noise statistic estimator for UKF:
(30)q^k=1k∑j=1k[x^j/j-∑i=0LWimfj-1(ξi,j-1/j-1)],(31)Q^k=1k∑j=1k{[x^j/j-x^j/j-1][x^j/j-x^j/j-1]T},(32)r^k=1k∑j=1k[zj-∑i=0LWimhj(ξi,j/j-1)],(33)R^k=1k∑j=1k{[zj-z^j/j-1][zj-z^j/j-1]T}.The unbiased properties of the noise estimates for UKF are proved in the appendix.


## 5. 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.(1) Prediction. Propagating the sigma points ξi,k/k-1 through nonlinear state function fk(·) yields
(34)γi,k/k-1=fk-1(ξi,k/k-1),i=0,1,…,2n.Then, according to (30) and (31), estimate the process noise q^k and covariance Q^k, respectively.With updated process noise parameters, compute the predicted statex^k/k-1 and the predicted covariance Pk/k-1as
(35)x^k/k-1=∑i=0LWimγi,k/k-1=∑i=0LWimfk-1(εi,k-1/k-1)+q^k-1,Pk/k-1=∑i=0LWim(γi,k-1/k-1-x^k/k-1)×(γi,k-1/k-1-x^k/k-1)T+Q^k-1.(2) Updating. Propagating the sigma points ξi,k/k-1 through nonlinear state function hk(·) yields
(36)xi,k/k-1=hk(ξi,k/k-1),i=0,1,…,2n.According to (32) and (33), estimate the measurement noise r^k and its covariance R^k, respectively.With real-time measurement noise parameters, compute the predicted measurementz^k/k-1 and the covariance Pz~k as
(37)z^k/k-1=∑i=0LWimxi,k/k-1=∑i=0LWimhk(εi,k/k-1)+r^k,(38)Pz~k=∑i=0LWim(χi,k/k-1-z^k/k-1)(χi,k/k-1-z^k/k-1)T+R^k.Estimate the cross-covariancePx~kz~k as
(39)Px~kz~k=∑i=0LWic(γi,k/k-1-x^k/k-1)(χi,k/k-1-z^k/k-1)T.Then compute the filter gainKk, the estimated state vector x^k/k, and its covariance Pk/k:
(40)Kk=Px~kz~k(Pz~k)-1,x^k/k=x^k/k-1+Kk(zk-z^k/k-1),Pk/k=Pk/k-1-KkPz~k(Kk)T.


## 6. 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 Section1 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 Table1. The initial position of vehicle is east longitude 126° and north latitude 45°.Table 1
Parameters of sensor errors.
Sensor
Characteristic
Value
MEMS Gyro
Drift
5°/h
MEMS Gyro
Measuring white noise
0.5°/h
MEMS accelerometer
Bias
2 mg
MEMS accelerometer
Measuring white noise
50μg
GPS (position)
Measuring white noise
6 m
GPS (velocity)
Measuring white noise
0.01 m/sFigure1 shows the trajectory of land-vehicle motion. The solid line in this figure illustrates the real simulated trajectory.Figure 1
The track of land vehicle.The architecture of MEMS/GPS integrated navigation with AUKF is shown in Figure2.Figure 2
The architecture of MEMS/GPS integrated system with AUKF.As shown in Figures3, 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, there 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.Comparison of position errors.
(a)
(b)Comparison of velocity errors.
(a)
(b)Comparison of attitude errors.
(a)
(b)
(c)Figure4 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.


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



---

*Source: 451939-2014-03-20.xml*