Square root unscented Kalman filter for MEMS gyroscope random drift compensation
ZHOU Jian-jun(周建军), SONG Chun-lei(宋春雷), ZHUANG Hui-hui(庄会慧)
(School of Automation, Beijing Institute of Technology, Beijing 100081, China)
Abstract: The micro electro mechanical system (MEMS) gyroscopes are widely used in many applications for its small size and low cost. They usually have deterministic system error and random error which can only be described by statistical models. First of all, its random drift error model is created by using the time-serial analysis, and then the process of decreasing this random drift error by making use of the square root unscented Kalman filter (SRUKF) based on the above error model is expounded. The compensating results for the practical testing data of a MEMS gyroscope show that the random drift error can be controlled effectively by the filtering method presented, and its application precision in practical system can be further proved.
Key words: MEMS gyroscope; SRUKF; random drift error; time-series analysis
CLC number: V241.5 Document code: A Article ID: 1672-7207(2011)S1-0469-04
1 Introduction
Recently, with the development of the micro electronics technology, MEMS inertial sensors have been improved greatly. Compared with conventional gyroscopes, MEMS gyroscope has a lot of good characteristics such as low cost, small size, light mass, mass production, long life, high reliability, low power consumption and so on, which make sensors are widely used in such fields as navigation, defense, medical instruments and consumer applications. The MEMS gyroscopes have a big potential to be used in future micro inertial system. However, the low precision property has seriously prevented its application in high accurate inertial systems. How to improve its application precision is not only a hot point which is attended by many experts, but also an application problem of practicality. We can effectively improve the accuracy of the MEMS gyro by analyzing the gyro error signal, and then modeling and compensation. They usually have deterministic system error which can be compensated by testing and demarcating and random error which can only be described by statistical models.
In this paper, the MEMS gyro random error model was created by using the time-serial analysis. However, most time series of practical relevance are complicated that makes conventional prediction methods inapplicable. In 1927, the mathematician Yule proposed autoregressive (AR) model to forecast time series. After several years, in 1931, another mathematician Walker invented moving average (MA) model and autoregressive moving average (ARMA) based on autoregressive (AR) model[1]. And in this study, ARMA (2, 1) model was used for parameter estimation.
Many methods have been developed to decrease this MEMS gyro random drift error. These methods include Kalman filter, robust filtering, nonlinear filtering and the enhancements and improvements. Of course, these methods can effectively solve the gyro’s random drift error compensation; especially have good work on the drift error model. Kalman filter can be used to deal with the above random error model, but it just is a filter for linear systems with no consideration of environmental noise. So in the case of serious non-linear, its model precision will be affected by divergence of filtering. In 1997, Julier and Uhlman proposed a new type of nonlinear filter, named unscented Kalman filter (UKF). It is based on the nonlinear Unscented Transformation (UT). Thereby, UKF does not use the linearization like kalman filter. UKF produces a set of selecting point (called sigma-point) in such a way that they together capture the full mean and covariance of the state and make the estimation based on these points. It provides an excellent solution for nonlinear system, but it is susceptible to possible divergence of covariance matrices due to accumulative runoff errors in the transition process. So the square root UKF (SRUKF) is proposed, it’s the square root filtering form of UKF. Square root UKF can not only guarantee the positive definite of covariance matrix, but also increase the accuracy of numerical calculation[2].
First, this article expounds the square root unscented transformation; the square root UKF principle and recursion steps. Based on these works, the square root UKF was designed. According to the practical testing data, the compensating results can be obtained by making use of the square root UKF, the random drift error can be controlled effectively by the filtering method presented, and its application precision in practical system can be further proved.
2 Square root unscented Kalman filter
2.1 Square root unscented transformation
The Unscented Transformation has been paid much attention within nonlinear filtering community for its significant accuracy and implementation advantages. By setting the sigma points and the corresponding weights, it can approach the first order matrix and second order matrix of the sample’s nonlinear transformation parameter. Let x be an nx-dimensional random variable with mean x and covariance Px, a second ny-dimensional random variable y is related to x through the nonlinear transformation y=g(x). With x and Px, sigma-point[3] and weight of x can be obtained.
Through the nonlinear transformation, new sigma-points are given:
(i=0, 1, …, 2nx) (1)
The mean and covariance of variable y are calculated as follows:
(2)
(3)
where 
and
are used to be weighted value.
Usually Py is a positive-definite matrix, but due to the impact of calculation error in iterative filtering, it will become a negative-definite matrix which leads to filter divergence. It will seriously affect filtering algorithm convergence speed and stability.
So, Py is decomposed:
(4)
The square root matrix Sy is got by Cholsky decomposition of Py, which will be obtained by QR decomposition in filtering update algorithm.
By setting the scaling parameters which can ensure the weight of the particle positive, we can get:
(5)
where qr is a function which will give us R matrix of QR decomposition. Due to using QR decomposition, Cholesky decomposition can be used before receiving the weighted variance. The above method can not only ensure the data stability, but also reduce the calculation steps. In order to guarantee the positive definite of covariance matrix, Sy instead of Py was used to calculate. This will make the filter algorithm very effective.
2.2 SRUKF Algorithm
(6)
(7)
where
is an nx-dimensional state vector; 
is an ny-dimensional observation vector; 
is an nx-dimensional process noise vector; 
is an ny-dimensional observation noise vector; uk is the input control vector. The mean of the process noise vk is 0; and its covariance matrix is
. The mean of the observation noise nk is 0, and its covariance
.
The SRUKF algorithm is given as follows:
(8)
(9)
For k=1, …, ∞.
Calculate sigma points:
(10)
where
; α determines the spread of sigma points and is usually set to a small positive value to avoid non-local effects; κ is a scaling parameter which is usually set to zero; 
is the ith column of 
.
Time update equations
(11)
(12)
(13)
(14)
(15)
(16)
The weights for mean and covariance are given:
(17)
where β is non-negative scaling parameter, usually for Gaussian distributions β=2 are optimal[4].
Measurement update equations
(18)
(19)
(20)
(21)
(22)
(23)
(24)
(25)
(26)
(27)
where chol, qr, cholupdate are the functions of Matlab. “/”represents the effective least squares pseudo-inverse with making use of the rotating triangular QR decomposition[4].
3 Numerical simulation
The output of MEMS gyro random drift is a continuous signal, but it must be a discrete signal which uses the time-serial analysis to create model. Therefore, for the purpose of getting a discrete signal we use a certain rate to sample the continuous signal before creating model[5]. The MEMS gyro was put on the level turntable and warm up in 0.5 h. Take a point per second at room temperature. The original drift signal of a MEMS gyro is shown in Fig.1. It is clear that the drift signal is contaminated by the strong noise signal.
Fig.1 Zero drift curve of MEMS gyro
The model of ARMA (2, 1) was used in Ref.[5-6], and its equation is as follows:
(28)
For
, the state equation of MEMS gyro random drift is:
(29)
where
,
.
For Y(k)=y(k), the observation equation is:
(30)
where C=[1 0].
Both Kalman filter and SRUKF were used in this model to filter the random noise. In order to illustrate advantage of SRUKF, Kalman filter was investigated with the same settings as a comparison in this work. The simulation results are shown in Fig.2 and Fig.3.
From Fig.2 and Fig.3, it can be seen that volatility of the drift data is greatly reduced after filtering; due to filtering, it is evident that the random noise is suppressed effectively. The variance of the original data is 0.037, but it becomes 0.003 2 based on Kalman filter and 0.000 61 based on SRUKF. From this simulation results, the variance is smaller and it is visible that the divergence of data is improved remarkably. Comparing Fig.2 to Fig.3, zero drift denoising with SRUKF is better than that with Kalman filter, and variance is less than that of Kalman filter.
Fig.2 Zero drift curves based on Kalman filter
Fig.3 Zero drift curves based on SRUKF
4 Conclusions
SRUKF is applied for the MEMS gyro random drift error compensation. The SRUKF sets sigma points to minimize the random item of noise in the MEMS gyro drift which increase the model accuracy. According to the simulation results, it can be seen that the performance of SRKUK is better than that of Kalman filter.
References
[1] WANG Jian-zhou, LIANG Jin-zhao, CHE Jin-xing, et al. ARMA model identification using particle swarm optimization algorithm[C]//International Conference on Computer Science and Information Technology, 2008: 223-227.
[2] LI Peng, SONG Shen-min, Duan G R. The square root Unscented Kalman filter in spacecrafts autonomous navigation[C]//ISSCAA 2008, 2th International Symposium on Systems and Control in Aerospace and Astronautics, 2008: 1-4.
[3] Julier S J, Uhlmann J K. Unscented filtering and nonlinear estimation[J]. Proceedings of the IEEE, 2004, 92(3): 401-422.
[4] TANG Xiao-jun, YAN Jie, ZHANG Du-du. Square-root sigma-point Kalman filtering for spacecraft relative navigation[J]. Acta Astronautica, 2010, 66(5/6):704-713.
[5] WANG Dong, WAN Bao-lin. Error analysis and model of MEMS gyro random drift[J]. China Water Transport, 2008, 11(8): 147-148.
[6] LI Jie, ZHANG Wen-dong, LIU Jun. Research on the application of the time-serial analysis based kalman filter in MEMS gyroscope random drift compensation[J]. Chinese Journal of Sensors and Actuators, 2006, 19(5): 2215-2219.
[7] LIN Zhao. Design of unscented Kalman filter with noise statistic estimator[J]. Control and Decision, 2009, 24(10): 1483-1488.
(Edited by ZHAO Jun)
Received date: 2011-04-15; Accepted date: 2011-06-15
Corresponding author: SONG Chun-lei, PhD, Associate Professor; Tel: +86-13520186893; E-mail: songchunlei@bit.edu.cn
