Drift Compensation of MEMS Gyro with Kalman Filtering Algorithm*

XU Han，ZENG Chao，HUANG Qinghua（1.School of Electronic and Optical Engineering，Nanjing University of Science and Technology，Nanjing 10094，China；.Institute of Electronic Engineering，China Academy of Engineering Physics，Mianyang Sichuan 61000，China）

Drift Compensation of MEMS Gyro with Kalman Filtering Algorithm*

XU Han1，2，ZENG Chao2*，HUANG Qinghua2
（1.School of Electronic and Optical Engineering，Nanjing University of Science and Technology，Nanjing 210094，China；2.Institute of Electronic Engineering，China Academy of Engineering Physics，Mianyang Sichuan 621000，China）

Aiming at solving the error of MEMS gyroscope's drift in the test，the kalman algorithm was proposed.In the thesis，MPU3050 gyroscope was analyzed，and its drift model was proposed.After the analysis of the cause of the error of the gyroscope，the error model of the gyroscope was established.The effect of kalman filter was evaluated on the Allan variance method.Compare with other filtering algorithm，kalman filtering algorithm which can reduce the burden of SCM（Single chip microcomputer）with a little computation，and the gyroscopes were tested on the rotating speed device.The experimental results show that the combination of the error of model and kalman filter algorithm can reduce drift of the gyroscope in the test.

MEMS gyroscope；Kalman filter；allan variance；drift error

EEACC:7630doi:10.3969/j.issn.1004-1699.2016.07.002

Pressure，abrasive particle size，slurry concentration and device error will affect the trajectory，which make the ball deviate from the expected trajectory during the ball lapping process.Deviation from spherical ball trajectory may affect the removal efficiency during the grinding process，reducing the machining accuracy［1-3］.

Trackball measurement system can obtain realtime gesture effectively in virtue of SINS，measurement of SINS need error compensation because of its dynamic error，Error sources from system which includes the inertial components installation error，measurement error of inertia components，initial alignment error and calculation error.The most serious errors is measurement error of inertia components，which mainly generated by the gyro drift［4-5］.

To aiming at the problem of gyroscope drift，wavelet algorithm was proposed to reducing noise of gyroscope by many scholars in recent years［6-7］.Wavelet algorithm is not suitable for embedded systems because of its complexity［8］.A novel Kalman filter algorithm for gyroscope noise reduction was proposed in this paper［9，10］.First gyro drift model was established，second a Kalman filtering algorithm was used to reduce the effect of random errors，and then the Kalman filtering algorithm was evaluated by analysis of Allan variance，atlast gyroscope measurement experiments was tested in the speed test platform［11］.

1　Gyroscope drift model

The gyro will have serious gyro drift in harsh conditions，the measurement error consist mainly of the scale factor error and gyro zero drift.According to the basic characteristics of MEMS gyro，simultaneous gyro drift static mathematical model was proposed：

Where：Nx，Ny，Nz-3-axis gyro measurement error；Ax，Ay，Az-3-axis gyro zero drift；Bx，By，Bz-3-axis gyro scale factor error；wx，wy，wz-3-axis gyro rotational speed.3-axis zero drift of gyroscope consists of zero constant error and zero random error.

Ax，Ay，Az-gyroscope constant error；ix，iy，izgyroscope random error；ΔAt-gyroscope temperature drift.

MEMS gyroscope MPU3050 of trackball measurement system was produce by Invensense Company.According to data enchiridion，the temperature drift of MPU3050 is 0.1°/（s/℃），temperature drift as follow：

The resolution of MPU3050 is 131 Lbs°/（s/℃）. The static nonlinearity is 0.2%［12］，so that MPU3050 need not measurement error compensation because of good linearity in a static environment.In a dynamic environment，sudden speed changes such as acceleration and jerk cause angular velocity error of MPU3050 increased.Angular velocity offset of MPU3050 is 0.1°/（s/℃），which caused by acceleration change.

Where：Bx′-dynamic scale factor error，Bx-static scale factor error，0.1a-offset compensation.Gyro drift mathematical model is proposed as follow：

2　Analysis and compensation of gyroscope zero drift

In this study，zero error of MPU3050 was compensated by Kalman filter algorithm.First，Kalman filter was used for noise reduction with stochastic signal，and then Allan variance analysis was used for comparing effect.

Zero error was consisted of the random error and constant error：

Where：J-constant error，S-random noise error，xi-Zero samples of point i，A-zero error.S is a zero mean random noise which was computed by the equation（6）.Kalman filter model is based on a known stochastic signal mathematical model which was proposed for time-varying non-stationary time series digital filter.Mathematical model of Kalman filter：

Where：xn-gyroscope state of point n；yn-gyroscope measurement of point n；wn-input noise；vn-observational noises；?n|n-1-state trasive matrix.Covariance ofwnandvnis Q，R respectively.Kalman filter recursive procedure is as follows：

One-step prediction error matrix as follows：

Gain factor：

Prediction error matrix as follows：

In this study，the random time series of before and after filtering of gyroscope was obtained by experiment. Specific experiments as follows：①The stationary gyro placed at a invariable 25℃indoor horizontal desktop.②Sampling period is 0.1 s，sample point N=1 000.③Compute gyroscope random error from 3-axes zero constant error of gyroscope.④Allan variance analysis was used for comparing random time series of gyroscope filtering before and after.

The x-axis gyro random noise has been significantly convergence and the amount of random drift is significantly reduced after Kalman filtering shows in Fig.1 and Fig.2.

Fig 1　Random sequences of x axis of gyro before Kalman filtering

Fig 2　Random sequences of x axis of gyro after Kalman filtering

The statistical characteristics of various error sources can be characterized and identified by analysis.It's an effective method to identify gyroscope random error，which can be applied to any random noise.

The sampling period ist0，sample frequency is m，correlation timet=kt0，the mean of correlation time t：

Assumed t is 0.1 s，0.2 s，0.4 s，1.0 s，2.0 s，4.0 s，10.0 s，50.0 s，respectively，and then comparing Allan variance of before and after Kalman filtering. Comparison of Allan variance of x axis of gyro is shown in Fig.3.

Fig.3　Allan variance of x axis of gyro（before and after Kalman filtering）

In Fig.3，the solid line is random error Allan variance before filtering，the dashed line is random error Allan variance after filtering，it's found that random errors Allan variance reduced obviously after Kalman filter，Which can prove that Kalman filtering for noise reduction is effective.Allan variance analysis can analyze the composition of the gyro noise.Random errors of MEMS gyroscope are consist of angle random walk（ARW），the rate of random walk（RRW），rate ramp walk（RRW），bias instability（BI），the quantization noise（QN），sinusoidal noise（SN）.Allan variance of MEMS gyroscope can be written as follows：

In this study，the coefficients ofAncan be obtained by formula（14）for using the least squares，namely that each part of the noise weighting factor for Allan variance，determine the impact of the noise source of random errors.

3　Experimental verification of gyro drift compensation model

Specific experimental procedures of gyro drift compensation model are as follows：Placed the gyro at ARMS turntable and power on，and x-axis of gyroscope parallel to the turntable.The turntable began to accelerate from 0，which was added to 10 r/min，a accelerate time is 100 s，a sampling period is 0.1 s，a total of N is 1 000 points，a temperature is 25℃.Uniform rotation time is 100 s，a sampling period is 0.1 s，a total of N 1 000 points.The uniform accelerationsequencediagram ofX-axisofgyroscope and the uniform rotation of sequence diagram are showninFig.4andFig.5respectively.

In uniform acceleration and uniform motion condition，the measurement results with gyro error compen-sation more accurate than which without gyro error compensation.

Fig 4　Time-ordered graph of uniform accelerating of x axis of gyro

Fig 5　Time-ordered graph of uniform rotation of x axis of gyro

4　Summary

In this study analyzed the characteristics of MPU3050 gyroscope and established drift model，and then research on Kalman filtering algorithm for random noise reduction of gyro，and use Allan variance analysis analyze effect of Kalman filtering for quantitative analysis.Experimental results show that the Kalman filtering algorithm can reduce random errors of gyroscope effectively，which improve the measurement accuracy of gyroscope.And compared to other algorithm，the computation burden of microcontrollers was reduced effectively by Kalman filtering algorithm with small calculation.

［1］ Huang C，Deng Q F.New Approach to Lapping Precision Ceramic Bearing Ball［J］.Light Industry Machinery，2008，26（6）：91-93.

［2］ Gao Z Y，Fang J J，Yu L J.Allan Variance Analysis For the Stochastic Error of MEMS Sensors［J］.Chinese Journal of Scientific Instrument，2011，32（12）：2863-2868.

［3］ Deng Y，Liu Z H，Li Q S.Estimation and Application of the Vehicles Running Status According to the Angle Rate Gyroscope［J］. Mechanical&Electrical Engineering Technology，2007，36（11）：83-86.

［4］ Li X Y，Zhang N.Analysis of Dynamic Characteristics of a Fiber-Optic Gyroscope Based on Dynamic Allan Variance［J］.Journal of Harbin Engineering University，2011，32（2）：183-187.

［5］ Lorenzo G，Patarizia T.The Dynamic Allan Variance［J］.Ultrasonics Ferroelectrics and Frequency Control，2009，52（3）：450-464.

［6］ Zhu S S，Ren J X A F.Zhang Random Walk Analysis of Fiber Optic Gyroscope［J］.Journal of Applied Optics，2009，30（6）：1003-1006.

［7］ Song N F，Zhang Z G，Li L J，Random Walk Coefficient of Fiber Optic Gyro［J］.Journal of Chinese Inertial Technology，2004，12（4）：34-38.

［8］ Zou X F，Lu X Y.Estimate Method of MEMS Gyroscope Performance Based on Allan Variance［J］.Micronanoelectronic Technology，2010，47（8）：490-493.

［9］ Song H B，Yang P，Xu L B.Analysis and Processing on Stochastic Error of MEMS Sensor［J］.Chinese Journal of sensors and actuators，2013，26（12）：1719-1723

［10］Mi G.Tian Z S.Jin Y.Li Z.Zhou M.MIMU Update Algorithm Based on the Posture and Magnetometer［J］.Chinese Journal of Sensors and Actuators，2015，28（1）：44-48

［11］Petoveuo M G.Performance Analysis of an Ultra-Tightly Integrated GPS and Reduced IMU System［J］.Proceedings of ION GNSS，2007，32（1）：802-815.

［12］Lashley M，Bevly D M，Hung J Y.Analysis of Deeply Integrated and Tightly Coupled Architectures［J］.Proceedings of IEEE/ION PLANS，2010，23（2）：382-396.

徐韓（1984-），男，漢族，四川人，南京理工大學博士研究生，主要研究方向為組合導航、MEMS器件，78365922@qq.com；

曾超（1968-）男，漢族，中國工程物理研究院研究員博士生導師，主要從事武器電子學系統科學，常規武器引信等領域，carlzeng@139.com。

基于卡爾曼濾波算法的MEMS陀螺儀誤差補償研究*

徐韓1，2，曾超2*，黃清華2
（1.南京理工大學電子工程與光電技術學院，南京210094；2.中國工程物理研究院電子工程研究所，四川綿陽621000）

為了補償MEMS陀螺儀的漂移誤差，本文采用了一種新的卡爾曼濾波算法。文中對MPU3050陀螺儀進行分析，提出其漂移模型。通過分析陀螺儀的誤差，建立陀螺儀的誤差模型。卡爾曼濾波的效果通過阿倫方差進行評估，與其他濾波算法比較，在MEMS陀螺儀中采用卡爾曼濾波算法可以有效的減少SCM（單片機）計算，并在轉臺上對陀螺儀進行測試。實驗結果表明，結合誤差模型和卡爾曼濾波算法可有有效的減少陀螺儀的漂移誤差。

MEMS陀螺儀；卡爾曼濾波器；阿倫方差；漂移誤差

TP212.9

A

1004-1699（2016）07-0962-04

2016-01-11修改日期：2016-01-15

項目來源：國防預先研究項目