- Email: [email protected]
A Noise Reduction Method for MEMS Gyroscope Based on Direct Modeling and Kalman Filter⁎
5th IFAC Conference on 5th IFAC Conference onControl, Simulation and Modeling Engine Powertrain 5th IFACand Conference on Engine and Powertrain Control, Simulation and Modeling Available at www.sciencedirect.com Changchun, China, September 2018 and online 5th IFACand Conference onControl,20-22, Engine Powertrain Simulation Modeling Changchun, China, September 20-22, 2018 Engine and Powertrain Control,20-22, Simulation Changchun, China, September 2018 and Modeling Changchun, China, September 20-22, 2018
ScienceDirect
IFAC PapersOnLine 51-31 (2018) 172–176
A Noise Reduction Method for MEMS A Noise Reduction Method for MEMS A Noise Reduction Method for MEMS Gyroscope Based on Direct Modeling and A Noise Reduction MethodModeling for MEMS Gyroscope Based on Direct and Gyroscope Based on Direct Modeling and Kalman Filter Gyroscope Based on Direct Modeling and Kalman Filter Kalman Filter Kalman Filter ∗∗∗ ∗∗∗∗ Shuo Cai ∗∗ Yunfeng Hu ∗∗ ∗∗ Haitao Ding ∗∗∗ Hong Chen ∗∗∗∗
Shuo Cai ∗∗ Yunfeng Hu ∗∗ ∗∗ Haitao Ding ∗∗∗ Hong Chen ∗∗∗∗ Shuo Cai Yunfeng Hu Haitao Ding ∗∗∗ Hong Chen ∗∗∗∗ ∗ ∗ Shuo Cai Yunfeng Hu ∗∗ Haitao Ding ∗∗∗ Hong Chen ∗∗∗∗ ∗ Department of Control Science and Engineering, Jilin University, ∗ Department of Control Science and Engineering, Jilin University, ∗ Department130025, of Control Science and [email protected]). Engineering, Jilin University, Changchun China (e-mail: ∗ Changchun 130025, China (e-mail: [email protected]). ∗∗ Department of Control Science and Engineering, JilinControl, University, State Key Laboratory of Automotive Simulation and Jilin Changchun 130025, China (e-mail: [email protected]). ∗∗ State Key Laboratory of Automotive Simulation and Control, Jilin ∗∗Changchun ∗∗ 130025, China (e-mail: [email protected]). University, Changchun China, (e-mail: [email protected]) State Key Laboratory of130025, Automotive Simulation and Control, Jilin University, Changchun China, (e-mail: [email protected]) ∗∗ ∗∗∗ State KeyofLaboratory of130025, Automotive Simulation and Control, Jilin University, Changchun 130025, China, (e-mail: [email protected]) College Automotive Engineering, Jilin University, Changchun ∗∗∗ College of Automotive Engineering, Jilin University, Changchun ∗∗∗ ∗∗∗ University, China, (e-mail: [email protected]) College130025, ofChangchun Automotive Engineering, Jilin University, Changchun China,130025, (e-mail: ding hai [email protected]) [email protected]) 130025, China, (e-mail: ding hai ∗∗∗ ∗∗∗∗ College of Laboratory Automotive Engineering, Jilin University, Changchun Key Automotive Simulation and 130025, China,of (e-mail: ding hai [email protected]) ∗∗∗∗ State Laboratory of Automotive Simulation and Control, Control, Jilin Jilin ∗∗∗∗ State Key ∗∗∗∗ 130025, China,130025, ding hai [email protected]) University, Changchun China, (e-mail: [email protected]) State Key Laboratory of(e-mail: Automotive Simulation and Control, Jilin University, Changchun 130025, China, (e-mail: [email protected]) ∗∗∗∗ State KeyChangchun Laboratory 130025, of Automotive and Control, Jilin University, China, Simulation (e-mail: [email protected]) University, Changchun 130025, China, (e-mail: [email protected]) Abstract: This paper focus on the topic of of reducing the the random noise noise in in the output output data data of Abstract: This focus topic of reducing Abstract: This paper paper focus on on the the reducing the random random noise in the theatoutput data of of MEMS gyroscope and improving thetopic accuracy of MEMS gyroscope. Aiming the problems MEMS gyroscope and improving thetopic accuracy of MEMS gyroscope. Aiming the problems Abstract: This paper focusareonnot the offor reducing thedata random noise in to theat output data of that thegyroscope previous methods suitable dynamic and difficult realize real-time MEMS and improving the accuracy of MEMS gyroscope. Aiming at the problems previous methods are notthe suitable for dynamic and difficult to at realize that thegyroscope real-time MEMS and improving accuracy of MEMSdata gyroscope. Aiming the problems online processing, aa direct modeling method proposed to establish the output data model that the previous methods are not suitable foris dynamic data and difficult to realize real-time online processing, direct modeling method is proposed to establish the output data model that the previous methods are foriskalman dynamic data and difficult to realize real-time of MEMS gyroscope. Basedmodeling on not this suitable model, the filter designed to output process the output online processing, a direct method proposed to is establish the data model of MEMS gyroscope. Based on this model, the kalman filter is designed to process the output online processing, a direct modeling method iskalman proposed to is establish the output model of MEMS gyroscope. Based on show this model, theproposed filter designed to process data theon output data. The experimental results that the method has good performance both data. The gyroscope. experimental results that the has good to performance both of MEMS Based on show this model, theproposed kalman method filter is designed process theon output static dataexperimental and dynamic data. data. The results show that the proposed method has good performance on both static data and dynamic data. data. static The dataexperimental and dynamicresults data. show that the proposed method has good performance on both © 2018,data IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. static and dynamic data. Keywords: MEMS gyroscope, Modeling, Kalman filter, Random noise, Angular rate rate Keywords: MEMS gyroscope, Keywords: MEMS gyroscope, Modeling, Modeling, Kalman Kalman filter, filter, Random Random noise, noise, Angular Angular rate Keywords: MEMS gyroscope, Modeling, Kalman filter, Random noise, Angular rate 1. INTRODUCTION filtering on the data processing (Xia et al. (2013), Ruan 1. filtering on the processing (Xia al. (2013), Ruan 1. INTRODUCTION INTRODUCTION filtering on (2014), the data data processing (Xia et etBai al. et (2013), Ruan and Y. U. Chen et al. (2012), al. (2013), and Y. U. (2014), Chen et al. (2012), Bai et al. (2013), 1. INTRODUCTION filtering on the data processing (Xia et al. (2013), Ruan and Y. U. (2014), Chen et al. (2012), Bai et al. (2013), Cao et al. (2015)). Support vector machine was applied MEMS gyroscope gyroscope is is the sensor sensor used used to to measure angular angular Cao et al. (2015)). Support vector machine was applied MEMS and Y. U. (2015)). (2014), Chen et al. (2012), Bai random etwas al. applied (2013), to the compensation of MEMS gyroscope noise MEMS gyroscope is the theinsensor usedcommercial, to measure measurecivil angular Cao et al. Support vector machine rate. It is widely used military, and to the of MEMS gyroscope random noise rate. It is used military, commercial, civil and et compensation al. (2015)). Support vector machineIn was MEMS gyroscope isits thein sensor used to measure angular (Shangguan et al. (2015), Li et al.gyroscope (2012)). the applied current rate. Itfields is widely widely used in military, commercial, civil and Cao to the compensation of MEMS random noise other due to small size, light weight, low cost (Shangguan et al. (2015), Li et al. (2012)). In the current other fields due to its small size, light weight, low cost the compensation of are MEMS random noise (Shangguan et al. Li based et al.gyroscope (2012)). the current rate.high Itfields isreliability. widely military, commercial, civil and to research, most of (2015), them on staticIn data, which other due toused its in small size, light weight, low cost and However, because of system structure research, most of them are based on static data, which and high reliability. However, because of system structure (Shangguan et al. (2015), Li et al. (2012)). In the current research, most of them are based on static data, which other fields due to its small size, light weight, low cost can only have good processing effects on static data and high reliability. However, because of system structure deviation, disturbance disturbance torque torque and and environmental environmental noise, noise, can only have good processing effects on static data and and deviation, research, most good of to them are based on on static data, which and high reliability. However, because ofleading system to structure are not dynamic data which is more general. deviation, disturbance torque anddata, environmental noise, onlyapplicable have processing effects static data and there is random noise in the output the low can are not applicable to dynamic data which is more general. there is random noise in the output data, leading to the low can not only have good effects on is static and deviation, disturbance torque and environmental noise, Some ofapplicable them apply complexdata algorithms that makes it there is random noise in the output data, leading toimprove the low are to processing dynamic which moredata general. accuracy of MEMS gyroscope. As a result, how to Some them apply complex algorithms it accuracy of gyroscope. As result, how improve are notof toreal-time dynamic which isthat moremakes general. ofapplicable them apply complexdata algorithms that makes it thereaccuracy is random noise in the output data, leading theissue low Some difficult to achieve online processing. To deal accuracy of MEMS MEMS gyroscope. As a ahas result, how ato toto improve the of MEMS gyroscope become hot difficult to achieve real-time online processing. To deal the accuracy of MEMS gyroscope has become a hot issue Some of them apply complex algorithms that makes it accuracy of MEMS gyroscope. As a result, how to improve with these problems, a direct modeling method is proposed the accuracy of MEMS gyroscope has become a hot issue difficult to achieve real-time online processing. To deal at home home and and abroad. with these a direct modeling method is proposed at toproblems, achieve real-time onlineofprocessing. To deal thehome accuracy of MEMS gyroscope has become a hot issue difficult to establish the output data model MEMS gyroscope. at and abroad. abroad. with these problems, a direct modeling method is proposed establish the output data modeling model of method MEMS isgyroscope. At present, there are mainly mainly two two methods methods to to reduce reduce the the to with these problems, a direct proposed at home andthere abroad. The model inthe this paper can model not only the static to establish output data of apply MEMStogyroscope. At present, are The model in this paper can not only apply to the At present, there are mainly two methods to reduce the random noise noise of of MEMS MEMS gyroscope. gyroscope. One One is is to to improve improve the to establish output data model of MEMS gyroscope. The model inthe this paper can output not only apply tosolving the static static data, but also to the dynamic data, thus the random but also to thepaper dynamic output data, thus solving the At present, there are gyroscope mainly twotomethods toimprove reduce the data, random noise of MEMS gyroscope. One is to structure of MEMS reduce the structural The model in this can not only apply to the static first problem. Then, the kalman filter is designed to process data, but also to the dynamic output data, thus solving the structure of MEMS gyroscope to reduce the structural problem. Then, the kalman filter is designed to process random noise of MEMS gyroscope. One is to the first structure of MEMS gyroscope to reduce theimprove structural error in hardware; The other is to design a filtering data, but alsodata. to theKalman dynamic output thus solving the the output filter is data, andesigned iterative first problem. Then, the kalman filter is to optimal process error in hardware; The other is to design a filtering output data. filter is is andesigned iterativeto optimal structure of gyroscope to the astructural error in hardware; The other noise is reduce toindesign filtering algorithm to MEMS eliminate random software (Jie and the first problem. Then,Kalman the kalman filter process estimation algorithm. The optimal at this moment the output data. Kalman filter isoutput an iterative optimal algorithm to eliminate random noise in software (Jie and algorithm. The at error in hardware; other is toindesign ais(Jie filtering algorithm to eliminate random software and estimation Zhang (2006)). In this thisThe paper, thenoise second method studied the output data. based Kalman filter isoutput an iterative optimal estimation algorithm. Theonoptimal optimal output at this this moment moment can be calculated the optimal estimate data of Zhang (2006)). In the second studied be calculated based on the optimal estimate data of algorithm tothe eliminate random in method softwareis and can Zhang (2006)). In this paper, paper, thenoise second method is(Jie studied to improve accuracy of MEMS gyroscope. estimation algorithm. The optimal output at this moment last moment and the measurement data of the current can be calculated based on the optimal estimate data of to improve the accuracy of MEMS gyroscope. moment and the measurement data of the current Zhang (2006)). this paper, the second method is studied last to improve the In accuracy of MEMS gyroscope. can be calculated based on the optimal estimate data of moment. Therefore, real-time online processing can be last moment and the measurement data of the current In the previous previous research, of theMEMS basic solution solution is to establish establish moment. Therefore, real-time online processing can be to improve the accuracy gyroscope. In the research, the basic is last and thereal-time measurement data of the tocurrent easilymoment achieved. Finally, experiments designed verify moment. Therefore, onlineare processing can be In the previous research, thebased basicon solution is to to establish the model of output data the existing MEMS easily achieved. Finally, experiments are designed to verify the model of datathebased on the existing MEMS Therefore, real-time be easily achieved. Finally, experiments areprocessing designed tocan verify the proposed method. The dataonline processing results show In the previous research, solution is toalgorithm establish the model of output output on the existing MEMS moment. gyroscope data, and data then based a basic specific filtering the proposed method. The data processing results show gyroscope data, and then a specific filtering algorithm easily achieved. Finally, experiments are designed to verify that the proposed method effectively reduces the random the proposed method. The data processing results show the model of output data based on the existing MEMS gyroscope and then specific filtering algorithm is designed designeddata, to process process the aoutput data to improve improve the that the proposed method effectively reduces the random is to the data to the method. Theeffectively data processing show drift.proposed that the proposed method reducesresults the random gyroscope data, and then specific filtering algorithm is designed to MEMS process the aoutput output data to improve the the accuracy of the gyroscope. A novel noise reduction drift. accuracy of the gyroscope. novel noise reduction drift.the proposed method effectively reduces the random is designed to MEMS process thecompressive outputA to improve the that accuracy ofcombined the MEMS gyroscope. A data novel noise reduction algorithm with sensing (CS) and algorithm with compressive sensing and 2. DIRECT MODELING OF MEMS GYROSCOPE accuracy ofcombined the transform MEMS gyroscope. A novel noise (CS) reduction algorithm combined with compressive sensing (CS) and drift. lifting wavelet (LWT) was proposed to reduce 2. OF lifting wavelet transform (LWT) was proposed to reduce 2. DIRECT DIRECT MODELING MODELING OF MEMS MEMS GYROSCOPE GYROSCOPE DATA algorithm combined with compressive sensing (CS) and lifting wavelet transform (LWT) was proposed to reduce the random noise (Zhu et al. (2014)). The ARMA model DATA the random noise (Zhu et al. (2014)). The ARMA model 2. DIRECT MODELING OF MEMS GYROSCOPE DATA lifting wavelet transform (LWT) to kalman reduce the random noise (Zhu etwas al. (2014)). The ARMA model was established, which usedwasfor forproposed applying was established, which was used applying kalman DATA In the previous methods, the model of MEMS the random noise which (Zhu etwas al. (2014)). ARMAkalman model In the previous methods, the model of MEMS gyroscope was established, used forThe applying gyroscope data mostlymethods, established time of series analysis based thewas previous thebymodel MEMS gyroscope was whichby was usedNatural for applying kalman In Thisestablished, work is supported National Science Foundation data was mostly established by time series analysis based This work is supported by National Natural Science Foundation In previous thebymodel of MEMS data was mostly established time series analysis based on the existing data,methods, which requires that the datagyroscope must be (NNSF) of China (No. 6179056461703177); JilinScience Provincial Science This work is supported by National Natural Foundation on existing data, which requires that the data must be (NNSF) of China (No. 6179056461703177); Jilin Provincial Science data was mostly established by time series analysis based steady, normal and zero mean. However, the dynamic data on existing data, which requires that the data must be Foundation of China and 20170520067JH); This work is supported by20180101037JC National Natural Science Foundation (NNSF) of China (No. (No. 6179056461703177); Jilin Provincial Science steady, normal and zero mean. However, the dynamic data Foundation of China (No. 20180101037JC and 20170520067JH); on existing data, which requires that the data must be Postdoctoral Science foundation of China (No. 2017M621209). do not meet the three properties above. This is the reason steady, normal and zero mean. However, the dynamic data (NNSF) of China (No. 6179056461703177); Jilin Provincial Science Foundation of China (No. 20180101037JC and 20170520067JH); Postdoctoral Science foundation of China (No. 2017M621209). do not meet theand three properties above. This is the reason steady, normal zero mean. However, the dynamic data Foundation Chinafoundation (No. 20180101037JC 20170520067JH); Postdoctoral of Science of China (No.and 2017M621209). do not meet the three properties above. This is the reason Postdoctoral Science foundation of China (No. 2017M621209). do not meet the three properties above. This is the reason 2405-8963 © © 2018 2018, IFAC (International Federation of Automatic Control) Copyright IFAC 195 Hosting by Elsevier Ltd. All rights reserved. Copyright 2018 IFAC 195 Control. Peer review© under responsibility of International Federation of Automatic Copyright © 2018 IFAC 195 10.1016/j.ifacol.2018.10.032 Copyright © 2018 IFAC 195
IFAC E-CoSM 2018 Changchun, China, September 20-22, 2018 Shuo Cai et al. / IFAC PapersOnLine 51-31 (2018) 172–176
why this method can not be applied to dynamic data very well. To deal with the problem above, this paper directly establishes a model that can be applied to most of the dynamic data of MEMS gyroscopes. The output of the MEMS gyroscope can be expressed as: (1) ωk = ωk−1 + ∆ωk−1 , (k = 1, 2, 3, ...)
173
3.2 Equations of Kalman filter Kalman filter is an iterative process. The filtering process in one cycle is as follows: One-step state prediction: ˆ k−1 ˆ k,k−1 = Φk,k−1 X (6) X
Where ωk is angular rate; ∆ωk is angular rate increment; k is a certain moment. The equation above shows that the output of the MEMS gyroscope at a certain moment is equal to the angular rate of the previous moment plus an angular rate increment. The difference between static data and dynamic data is whether ∆ωk is equal to 0◦ /s.
One-step prediction mean square error:
The difficulty in modeling for dynamic data is that the value of ∆ωk and how it changes as time goes on are unknown. So this paper assumes that the angular rate increment at k is equal to the angular rate increment at k−1: (2) ∆ωk = ∆ωk−1 .(k = 1, 2, 3, ...)
State update:
Combining equations (1) and (2), the model of MEMS gyroscope output data is obtained as follow: 11 ωk−1 ωk = , (k = 1, 2, 3...) (3) 01 ∆ωk ∆ωk−1 Angular velocity is a state of the object being measured. For a MEMS gyroscope, angular velocity is an input. And how it changes is completely unknown and unpredictable. The above model is a relatively reasonable one for the output data of gyroscope. 3. DESIGN OF KALMAN FILTER The kalman filter algorithm was first proposed by R.E.Kalman in 1960 (Kalman (1960)). Kalman filter is an optimal estimation method that is essentially different from conventional filtering methods. It applies information estimation theory to process the data. This method is available for estimating one or more dimensional random processes which can be either stationary or non-stationary. Because the noise signal of the MEMS gyroscope is random, kalman filter can be used to process the output data to improve its accuracy.
T Pk,k−1 = Φk,k−1 Pk−1 ΦT k,k−1 + Γk,k−1 Qk−1 Γk,k−1
Kalman filter gain: −1 Kk = Pk,k−1 HkT Hk Pk,k−1 HkT + Rk
There is a linear discrete stochastic system, assuming that its system equation is shown in equation (4) and its measurement equation is shown in equation (5), Xk = Φk,k−1 Xk−1 + Γk,k−1 Wk−1 (4) (5) Zk = Hk Xk + Vk Where Xk is the state variable of the system; Φk,k−1 is the state-transition matrix; Wk is the process noise; Γk,k−1 is the noise input matrix; Zk is the measurement vector; Hk is the measurement matrix; Vk is the measurement noise. The process noise Wk and the measurement noise Vk are Gaussian white noise. The covariance of Wk is Q and that of Vk is R. If the linear discrete stochastic system satisfies the above conditions, the state can be estimated by the kalman filter algorithm. 196
(8)
ˆ k,k−1 + Kk Zk − Hk X ˆk = X ˆ k,k−1 X
(9)
Pk = [I − Kk Hk ] Pk,k−1
(10)
Posterior estimation mean square error:
Output update: ˆk Zˆk = Hk X
(11)
ˆ k,k−1 is the priori state estimate which is Where X ˆ k is the predicted according to the previous state; X posterior state estimate which is the correction of the priori estimate by the measurement data Zk ; Pk,k−1 is the priori estimate mean square error; Kk is kalman filter gain which ˆ k is ˆ k,k−1 ; X is used to correct the priori state estimate X ˆ the posterior estimate mean square error; Zk is the output of kalman filter; I is the identity matrix. The process of kalman filter is shown in figure 1. m
)
)
3.1 State Space Equation of Kalman Filter
(7)
*
m
Fig. 1. The Process of Kalman Filter (Xia et al. (2013)) In section 2, a model of the MEMS gyroscope output data has been obtained. Since there is random noise in the measured angular rate, it can be expressed in the form of equations (4) and (5) as follows: 1 1 ωk−1 ωk (12) = + Wk−1 0 1 ∆ωk ∆ωk−1
IFAC E-CoSM 2018 174 Changchun, China, September 20-22, 2018 Shuo Cai et al. / IFAC PapersOnLine 51-31 (2018) 172–176
ω k ∆ ωk
=
10 01
ωk ∆ωk
+ Vk
(13)
The state variables are angular rate and angular rate ωk difference, that is Xk = . The measurement vector ∆ωk ω k is Zk = . The state-transition matrix is Φk,k−1 = ∆ ωk 11 . The noise input matrix is Γk,k−1 = 1. The 01 10 measurement matrix is Hk = . 01 Applying kalman filter to MEMS gyroscopes, Q and R are required. The values of Q and R are set based on 0.0001 0 experience. In this paper, Q is set to , and 0 0.0001 10 R is set to . In order to activate kalman filtering, the 01 ˆ 0 and that of initial value of the posterior state estimate X the posterior estimate P0 are required. mean square error 0.1 0 0.1 ˆ 0 is set to in this , and P0 is set to X 0 0.1 0.1 paper.
Fig. 2. Communication Schematic of Data Acquisition System before and after filtering is shown in figure 3 and figure 4. Limited to space, the figures show only 100 seconds of the first two sets of data. The blue line is the original static data, and the red line is the filtered data. It can be seen from the figure that the drift noise of MEMS gyroscope is reduced. The variance of static data before and after filtering is given in table 1. As can be seen from the table, the variance of static data is reduced by 90% after filtering. So it is proved that the method proposed in this paper has a good performance on the static data.
4. TESTING AND RESULTS
4.1 Data Acquisition System The data acquisition system includes MEMS gyroscope, PC, control chip, power circuit, clock circuit and reset circuit. The control chip used in this paper is STC89C52, and the MEMS gyroscope is MPU6050. The measurement range of MPU6050 is set to ±250◦ /s. MPU6050 passes the measured data to STC89C52RC, and the PC reads the data from STC89C52RC through the serial debugging software XCOM. The communication protocol between STC89C52RC and MPU6050 is IIC protocol, and that between XCOM and STC89C52RC is RS232 protocol. The communication schematic of data acquisition system is shown in figure 2 . 4.2 Static test In the static test, four sets of data were collected when the MPU6050 was stationary, that is the input angular rate of MEMS gyroscope is 0◦ /s. The collection time for each set of data is one hour. There is a constant drift in the collected data which should first be removed. Because the constant drift can not be measured, the average of the four sets of static data is used to estimate the constant drift, and its value is −0.8375◦ /s. Then, it is subtracted from all the collected data. This is also needed when dealing with the dynamic data. Kalman filter is applied to the processed data above according to section 3. The comparison of static data 197
6
Static data Static data after filtering
4 Angular rate ω (°/s)
In order to verify whether the method proposed in this paper can be used to reduce the random noise, a data acquisition system is built to get the experimental data and the static and dynamic tests were carried out.
2 0 −2 −4 −6 0
20
40 60 Time t(s)
80
100
Fig. 3. Comparison of Static Data Before and After Filtering (Set 1) Table 1. Variance of Static Data Before and After Filtering Data Set Set 1 Set 2 Set 3 Set 4
Before Filtering 1.9612 1.9450 1.9604 1.9745
After Filtering 0.1718 0.1687 0.1707 0.1643
4.3 Dynamic Testing In the dynamic test, both the simulative dynamic data and the real dynamic data are used to verify the effectiveness of the proposed method.
IFAC E-CoSM 2018 Changchun, China, September 20-22, 2018 Shuo Cai et al. / IFAC PapersOnLine 51-31 (2018) 172–176
6
2 0 −2 −4 −6 0
Differential data after filtering Sine differential data
0.3 Differential data ∆ω (°/s)
Angular rate ω (°/s)
0.4
Static data Static data after filtering
4
175
0.2 0.1 0 −0.1 −0.2 −0.3
20
40 60 Time t(s)
80
−0.4 0
100
20
40 60 Time t(s)
80
100
Fig. 4. Comparison of Static Data Before and After Filtering (Set 2)
Fig. 6. Comparison of Simulative Dynamic Difference Data Before and After Filtering
Because the actual input angular rate in the dynamic data is unknown, which will make it difficult to judge the filtering effect, a sinusoidal signal is added to the static data in section 4.2 to simulate the dynamic data. The sinusoidal signal simulates the real input angular rate. Kalman filter is applied to the simulative dynamic data, and the parameters are set according to section 3. The filtering performance of simulative dynamic data is shown in figure 5 and figure 6. The mean square error of simulative dynamic data before and after filtering is given in table 2.
filtering, and the filtered data (red line) almost coincides with the simulative real input angular rate (green line). Because the differential data of the simulative dynamic data is too large, it is not given in figure 6. As can be seen, the method also has good performance on differential data. It can be seen from table 2 that the mean square error of four sets of simulative dynamic data is reduced by 90% after filtering. So it proves that the method proposed in this paper has good performance on simulative dynamic data.
40
Simulative Dynamic data Simulative Dynamic data after filtering Sine data
20
300
10
−10 −20 −30 0
Dynamic data Dynamic data after filtering
200
0 Angular rate ω (°/s)
Angular rate ω (°/s)
30
In order to verify the availability of the proposed method to the real dynamic data, four sets of dynamic data are collected, and the proposed filter algorithm is applied to the four sets of data. The filtering effect is shown in figure 7 and figure 8, and the variance of differential data before and after filtering is given in table 3.
20
40 60 Time t(s)
80
100
Fig. 5. Comparison of Simulative Dynamic Data Before and After Filtering
Before Filtering 1.9612 1.9450 1.9604 1.9745
0 −100 −200 −300 0
Table 2. Mean Square Error of Simulative Dynamic Data Before and After Filtering Data Set Set 1 Set 2 Set 3 Set 4
100
5
Time t(s)
10
15
Fig. 7. Comparison of Dynamic Data Before and After Filtering (Set 1)
After Filtering 0.1821 0.1810 0.1819 0.1751
As can be seen in Figure 5, the random noise in the simulative dynamic data is significantly reduced after 198
As can be seen from figure 7 and figure 8, the trend of the filtered data(red line) is consistent with that of the data before filtering(blue line), and the oscillation is significantly reduced. The data in table 3 shows that the variance of dynamic difference data is reduced by about
IFAC E-CoSM 2018 176 Changchun, China, September 20-22, 2018 Shuo Cai et al. / IFAC PapersOnLine 51-31 (2018) 172–176
300
Dynamic data Dynamic data after filtering
Angular rate ω (°/s)
200 100 0 −100 −200 0
5
Time t(s)
10
15
Fig. 8. Comparison of Dynamic Data Before and After Filtering (Set 2) 90% after filtering. So it proves that the proposed method also has good effect on real dynamic data. Table 3. Variance of Dynamic Difference Data Before and After Filtering Data Set Set 1 Set 2 Set 3 Set 4
Before Filtering 616.7533 985.9353 369.3168 658.2255
After Filtering 77.5989 74.7044 34.9843 73.7839
Compared with the AR model in previous studies(Xia et al. (2013), et al.), the model in this paper is not only applicable to static data, but also to dynamic data. The experimental results show that the method in this paper has good filtering effect. 5. CONCLUSION In order to reduce the random noise in the output data of MEMS gyroscope and improve the accuracy of MEMS gyroscope, this paper proposes a direct modeling method to establish the output data model. Different from previous data-driven approaches, the method in this paper is a mechanism modeling method. Therefore, the model is suitable for both static data and dynamic data of MEMS gyroscope. Based on this model, the kalman filter is designed to process the output data. Then the static data test and the dynamic data test are carried on. The experimental results show that the proposed method has good performance on both static data and dynamic data. REFERENCES Bai, J., Chen, W., and Cai, T. (2013). Compensation for mems gyroscope zero bias stability. In Chinese Automation Congress (CAC), 744–748. Cao, H., Lv, H., and Sun, Q. (2015). Model design based on mems gyroscope random error. In IEEE International Conference on Information and Automation, 2176–2181. Chen, W.C., Gao, G.W., Wang, J., Liu, L.L., and Li, X.L. (2012). The study of the mems gyro zero drift signal based on the adaptive kalman filter. In International Conference on Remote Sensing, 635–639. 199
Jie, L.I. and Zhang, W.D. (2006). Research on the application of the time-serial analysis based kalman filter in mems gyroscope random drift compensation. Chinese Journal of Sensors & Actuators, 19, 2215–2219. Kalman, R.E. (1960). A new approach to linear filtering and prediction problems. Journal of basic Engineering, 82(1), 35–45. Li, Z., Duan, F., and Ma, J. (2012). Random drift compensation of mems gyroscope based on support vector machine. Chinese Journal of Sensors & Actuators, 25(8), 1084–1087. Ruan, X.G. and Y. U., M.M. (2014). Modeling research of mems gyro drift based on kalman filter. In Chinese Control and Decision Conference, 2949–2952. Shangguan, Y., Chen, J., Song, C., and Han, Y. (2015). Research on the compensation in mems gyroscope random drift based on time-series analysis and kalman filtering. In Chinese Control Conference, 2078–2082. Xia, Y., Chen, W., and Peng, W. (2013). Research on random drift modeling and a kalman filter based on the differential signal of mems gyroscope. In Chinese Control and Decision Conference, 3233–3237. Zhu, W.J., Wang, G.L., Qiao, Z.T., and Gao, F.Q. (2014). A novel noise reduction algorithm of mems gyroscope based on compressive sensing and lifting wavelet transform. Key Engineering Materials, 609610(609-610), 1138–1143.
ScienceDirect
IFAC PapersOnLine 51-31 (2018) 172–176
A Noise Reduction Method for MEMS A Noise Reduction Method for MEMS A Noise Reduction Method for MEMS Gyroscope Based on Direct Modeling and A Noise Reduction MethodModeling for MEMS Gyroscope Based on Direct and Gyroscope Based on Direct Modeling and Kalman Filter Gyroscope Based on Direct Modeling and Kalman Filter Kalman Filter Kalman Filter ∗∗∗ ∗∗∗∗ Shuo Cai ∗∗ Yunfeng Hu ∗∗ ∗∗ Haitao Ding ∗∗∗ Hong Chen ∗∗∗∗
Shuo Cai ∗∗ Yunfeng Hu ∗∗ ∗∗ Haitao Ding ∗∗∗ Hong Chen ∗∗∗∗ Shuo Cai Yunfeng Hu Haitao Ding ∗∗∗ Hong Chen ∗∗∗∗ ∗ ∗ Shuo Cai Yunfeng Hu ∗∗ Haitao Ding ∗∗∗ Hong Chen ∗∗∗∗ ∗ Department of Control Science and Engineering, Jilin University, ∗ Department of Control Science and Engineering, Jilin University, ∗ Department130025, of Control Science and [email protected]). Engineering, Jilin University, Changchun China (e-mail: ∗ Changchun 130025, China (e-mail: [email protected]). ∗∗ Department of Control Science and Engineering, JilinControl, University, State Key Laboratory of Automotive Simulation and Jilin Changchun 130025, China (e-mail: [email protected]). ∗∗ State Key Laboratory of Automotive Simulation and Control, Jilin ∗∗Changchun ∗∗ 130025, China (e-mail: [email protected]). University, Changchun China, (e-mail: [email protected]) State Key Laboratory of130025, Automotive Simulation and Control, Jilin University, Changchun China, (e-mail: [email protected]) ∗∗ ∗∗∗ State KeyofLaboratory of130025, Automotive Simulation and Control, Jilin University, Changchun 130025, China, (e-mail: [email protected]) College Automotive Engineering, Jilin University, Changchun ∗∗∗ College of Automotive Engineering, Jilin University, Changchun ∗∗∗ ∗∗∗ University, China, (e-mail: [email protected]) College130025, ofChangchun Automotive Engineering, Jilin University, Changchun China,130025, (e-mail: ding hai [email protected]) [email protected]) 130025, China, (e-mail: ding hai ∗∗∗ ∗∗∗∗ College of Laboratory Automotive Engineering, Jilin University, Changchun Key Automotive Simulation and 130025, China,of (e-mail: ding hai [email protected]) ∗∗∗∗ State Laboratory of Automotive Simulation and Control, Control, Jilin Jilin ∗∗∗∗ State Key ∗∗∗∗ 130025, China,130025, ding hai [email protected]) University, Changchun China, (e-mail: [email protected]) State Key Laboratory of(e-mail: Automotive Simulation and Control, Jilin University, Changchun 130025, China, (e-mail: [email protected]) ∗∗∗∗ State KeyChangchun Laboratory 130025, of Automotive and Control, Jilin University, China, Simulation (e-mail: [email protected]) University, Changchun 130025, China, (e-mail: [email protected]) Abstract: This paper focus on the topic of of reducing the the random noise noise in in the output output data data of Abstract: This focus topic of reducing Abstract: This paper paper focus on on the the reducing the random random noise in the theatoutput data of of MEMS gyroscope and improving thetopic accuracy of MEMS gyroscope. Aiming the problems MEMS gyroscope and improving thetopic accuracy of MEMS gyroscope. Aiming the problems Abstract: This paper focusareonnot the offor reducing thedata random noise in to theat output data of that thegyroscope previous methods suitable dynamic and difficult realize real-time MEMS and improving the accuracy of MEMS gyroscope. Aiming at the problems previous methods are notthe suitable for dynamic and difficult to at realize that thegyroscope real-time MEMS and improving accuracy of MEMSdata gyroscope. Aiming the problems online processing, aa direct modeling method proposed to establish the output data model that the previous methods are not suitable foris dynamic data and difficult to realize real-time online processing, direct modeling method is proposed to establish the output data model that the previous methods are foriskalman dynamic data and difficult to realize real-time of MEMS gyroscope. Basedmodeling on not this suitable model, the filter designed to output process the output online processing, a direct method proposed to is establish the data model of MEMS gyroscope. Based on this model, the kalman filter is designed to process the output online processing, a direct modeling method iskalman proposed to is establish the output model of MEMS gyroscope. Based on show this model, theproposed filter designed to process data theon output data. The experimental results that the method has good performance both data. The gyroscope. experimental results that the has good to performance both of MEMS Based on show this model, theproposed kalman method filter is designed process theon output static dataexperimental and dynamic data. data. The results show that the proposed method has good performance on both static data and dynamic data. data. static The dataexperimental and dynamicresults data. show that the proposed method has good performance on both © 2018,data IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. static and dynamic data. Keywords: MEMS gyroscope, Modeling, Kalman filter, Random noise, Angular rate rate Keywords: MEMS gyroscope, Keywords: MEMS gyroscope, Modeling, Modeling, Kalman Kalman filter, filter, Random Random noise, noise, Angular Angular rate Keywords: MEMS gyroscope, Modeling, Kalman filter, Random noise, Angular rate 1. INTRODUCTION filtering on the data processing (Xia et al. (2013), Ruan 1. filtering on the processing (Xia al. (2013), Ruan 1. INTRODUCTION INTRODUCTION filtering on (2014), the data data processing (Xia et etBai al. et (2013), Ruan and Y. U. Chen et al. (2012), al. (2013), and Y. U. (2014), Chen et al. (2012), Bai et al. (2013), 1. INTRODUCTION filtering on the data processing (Xia et al. (2013), Ruan and Y. U. (2014), Chen et al. (2012), Bai et al. (2013), Cao et al. (2015)). Support vector machine was applied MEMS gyroscope gyroscope is is the sensor sensor used used to to measure angular angular Cao et al. (2015)). Support vector machine was applied MEMS and Y. U. (2015)). (2014), Chen et al. (2012), Bai random etwas al. applied (2013), to the compensation of MEMS gyroscope noise MEMS gyroscope is the theinsensor usedcommercial, to measure measurecivil angular Cao et al. Support vector machine rate. It is widely used military, and to the of MEMS gyroscope random noise rate. It is used military, commercial, civil and et compensation al. (2015)). Support vector machineIn was MEMS gyroscope isits thein sensor used to measure angular (Shangguan et al. (2015), Li et al.gyroscope (2012)). the applied current rate. Itfields is widely widely used in military, commercial, civil and Cao to the compensation of MEMS random noise other due to small size, light weight, low cost (Shangguan et al. (2015), Li et al. (2012)). In the current other fields due to its small size, light weight, low cost the compensation of are MEMS random noise (Shangguan et al. Li based et al.gyroscope (2012)). the current rate.high Itfields isreliability. widely military, commercial, civil and to research, most of (2015), them on staticIn data, which other due toused its in small size, light weight, low cost and However, because of system structure research, most of them are based on static data, which and high reliability. However, because of system structure (Shangguan et al. (2015), Li et al. (2012)). In the current research, most of them are based on static data, which other fields due to its small size, light weight, low cost can only have good processing effects on static data and high reliability. However, because of system structure deviation, disturbance disturbance torque torque and and environmental environmental noise, noise, can only have good processing effects on static data and and deviation, research, most good of to them are based on on static data, which and high reliability. However, because ofleading system to structure are not dynamic data which is more general. deviation, disturbance torque anddata, environmental noise, onlyapplicable have processing effects static data and there is random noise in the output the low can are not applicable to dynamic data which is more general. there is random noise in the output data, leading to the low can not only have good effects on is static and deviation, disturbance torque and environmental noise, Some ofapplicable them apply complexdata algorithms that makes it there is random noise in the output data, leading toimprove the low are to processing dynamic which moredata general. accuracy of MEMS gyroscope. As a result, how to Some them apply complex algorithms it accuracy of gyroscope. As result, how improve are notof toreal-time dynamic which isthat moremakes general. ofapplicable them apply complexdata algorithms that makes it thereaccuracy is random noise in the output data, leading theissue low Some difficult to achieve online processing. To deal accuracy of MEMS MEMS gyroscope. As a ahas result, how ato toto improve the of MEMS gyroscope become hot difficult to achieve real-time online processing. To deal the accuracy of MEMS gyroscope has become a hot issue Some of them apply complex algorithms that makes it accuracy of MEMS gyroscope. As a result, how to improve with these problems, a direct modeling method is proposed the accuracy of MEMS gyroscope has become a hot issue difficult to achieve real-time online processing. To deal at home home and and abroad. with these a direct modeling method is proposed at toproblems, achieve real-time onlineofprocessing. To deal thehome accuracy of MEMS gyroscope has become a hot issue difficult to establish the output data model MEMS gyroscope. at and abroad. abroad. with these problems, a direct modeling method is proposed establish the output data modeling model of method MEMS isgyroscope. At present, there are mainly mainly two two methods methods to to reduce reduce the the to with these problems, a direct proposed at home andthere abroad. The model inthe this paper can model not only the static to establish output data of apply MEMStogyroscope. At present, are The model in this paper can not only apply to the At present, there are mainly two methods to reduce the random noise noise of of MEMS MEMS gyroscope. gyroscope. One One is is to to improve improve the to establish output data model of MEMS gyroscope. The model inthe this paper can output not only apply tosolving the static static data, but also to the dynamic data, thus the random but also to thepaper dynamic output data, thus solving the At present, there are gyroscope mainly twotomethods toimprove reduce the data, random noise of MEMS gyroscope. One is to structure of MEMS reduce the structural The model in this can not only apply to the static first problem. Then, the kalman filter is designed to process data, but also to the dynamic output data, thus solving the structure of MEMS gyroscope to reduce the structural problem. Then, the kalman filter is designed to process random noise of MEMS gyroscope. One is to the first structure of MEMS gyroscope to reduce theimprove structural error in hardware; The other is to design a filtering data, but alsodata. to theKalman dynamic output thus solving the the output filter is data, andesigned iterative first problem. Then, the kalman filter is to optimal process error in hardware; The other is to design a filtering output data. filter is is andesigned iterativeto optimal structure of gyroscope to the astructural error in hardware; The other noise is reduce toindesign filtering algorithm to MEMS eliminate random software (Jie and the first problem. Then,Kalman the kalman filter process estimation algorithm. The optimal at this moment the output data. Kalman filter isoutput an iterative optimal algorithm to eliminate random noise in software (Jie and algorithm. The at error in hardware; other is toindesign ais(Jie filtering algorithm to eliminate random software and estimation Zhang (2006)). In this thisThe paper, thenoise second method studied the output data. based Kalman filter isoutput an iterative optimal estimation algorithm. Theonoptimal optimal output at this this moment moment can be calculated the optimal estimate data of Zhang (2006)). In the second studied be calculated based on the optimal estimate data of algorithm tothe eliminate random in method softwareis and can Zhang (2006)). In this paper, paper, thenoise second method is(Jie studied to improve accuracy of MEMS gyroscope. estimation algorithm. The optimal output at this moment last moment and the measurement data of the current can be calculated based on the optimal estimate data of to improve the accuracy of MEMS gyroscope. moment and the measurement data of the current Zhang (2006)). this paper, the second method is studied last to improve the In accuracy of MEMS gyroscope. can be calculated based on the optimal estimate data of moment. Therefore, real-time online processing can be last moment and the measurement data of the current In the previous previous research, of theMEMS basic solution solution is to establish establish moment. Therefore, real-time online processing can be to improve the accuracy gyroscope. In the research, the basic is last and thereal-time measurement data of the tocurrent easilymoment achieved. Finally, experiments designed verify moment. Therefore, onlineare processing can be In the previous research, thebased basicon solution is to to establish the model of output data the existing MEMS easily achieved. Finally, experiments are designed to verify the model of datathebased on the existing MEMS Therefore, real-time be easily achieved. Finally, experiments areprocessing designed tocan verify the proposed method. The dataonline processing results show In the previous research, solution is toalgorithm establish the model of output output on the existing MEMS moment. gyroscope data, and data then based a basic specific filtering the proposed method. The data processing results show gyroscope data, and then a specific filtering algorithm easily achieved. Finally, experiments are designed to verify that the proposed method effectively reduces the random the proposed method. The data processing results show the model of output data based on the existing MEMS gyroscope and then specific filtering algorithm is designed designeddata, to process process the aoutput data to improve improve the that the proposed method effectively reduces the random is to the data to the method. Theeffectively data processing show drift.proposed that the proposed method reducesresults the random gyroscope data, and then specific filtering algorithm is designed to MEMS process the aoutput output data to improve the the accuracy of the gyroscope. A novel noise reduction drift. accuracy of the gyroscope. novel noise reduction drift.the proposed method effectively reduces the random is designed to MEMS process thecompressive outputA to improve the that accuracy ofcombined the MEMS gyroscope. A data novel noise reduction algorithm with sensing (CS) and algorithm with compressive sensing and 2. DIRECT MODELING OF MEMS GYROSCOPE accuracy ofcombined the transform MEMS gyroscope. A novel noise (CS) reduction algorithm combined with compressive sensing (CS) and drift. lifting wavelet (LWT) was proposed to reduce 2. OF lifting wavelet transform (LWT) was proposed to reduce 2. DIRECT DIRECT MODELING MODELING OF MEMS MEMS GYROSCOPE GYROSCOPE DATA algorithm combined with compressive sensing (CS) and lifting wavelet transform (LWT) was proposed to reduce the random noise (Zhu et al. (2014)). The ARMA model DATA the random noise (Zhu et al. (2014)). The ARMA model 2. DIRECT MODELING OF MEMS GYROSCOPE DATA lifting wavelet transform (LWT) to kalman reduce the random noise (Zhu etwas al. (2014)). The ARMA model was established, which usedwasfor forproposed applying was established, which was used applying kalman DATA In the previous methods, the model of MEMS the random noise which (Zhu etwas al. (2014)). ARMAkalman model In the previous methods, the model of MEMS gyroscope was established, used forThe applying gyroscope data mostlymethods, established time of series analysis based thewas previous thebymodel MEMS gyroscope was whichby was usedNatural for applying kalman In Thisestablished, work is supported National Science Foundation data was mostly established by time series analysis based This work is supported by National Natural Science Foundation In previous thebymodel of MEMS data was mostly established time series analysis based on the existing data,methods, which requires that the datagyroscope must be (NNSF) of China (No. 6179056461703177); JilinScience Provincial Science This work is supported by National Natural Foundation on existing data, which requires that the data must be (NNSF) of China (No. 6179056461703177); Jilin Provincial Science data was mostly established by time series analysis based steady, normal and zero mean. However, the dynamic data on existing data, which requires that the data must be Foundation of China and 20170520067JH); This work is supported by20180101037JC National Natural Science Foundation (NNSF) of China (No. (No. 6179056461703177); Jilin Provincial Science steady, normal and zero mean. However, the dynamic data Foundation of China (No. 20180101037JC and 20170520067JH); on existing data, which requires that the data must be Postdoctoral Science foundation of China (No. 2017M621209). do not meet the three properties above. This is the reason steady, normal and zero mean. However, the dynamic data (NNSF) of China (No. 6179056461703177); Jilin Provincial Science Foundation of China (No. 20180101037JC and 20170520067JH); Postdoctoral Science foundation of China (No. 2017M621209). do not meet theand three properties above. This is the reason steady, normal zero mean. However, the dynamic data Foundation Chinafoundation (No. 20180101037JC 20170520067JH); Postdoctoral of Science of China (No.and 2017M621209). do not meet the three properties above. This is the reason Postdoctoral Science foundation of China (No. 2017M621209). do not meet the three properties above. This is the reason 2405-8963 © © 2018 2018, IFAC (International Federation of Automatic Control) Copyright IFAC 195 Hosting by Elsevier Ltd. All rights reserved. Copyright 2018 IFAC 195 Control. Peer review© under responsibility of International Federation of Automatic Copyright © 2018 IFAC 195 10.1016/j.ifacol.2018.10.032 Copyright © 2018 IFAC 195
IFAC E-CoSM 2018 Changchun, China, September 20-22, 2018 Shuo Cai et al. / IFAC PapersOnLine 51-31 (2018) 172–176
why this method can not be applied to dynamic data very well. To deal with the problem above, this paper directly establishes a model that can be applied to most of the dynamic data of MEMS gyroscopes. The output of the MEMS gyroscope can be expressed as: (1) ωk = ωk−1 + ∆ωk−1 , (k = 1, 2, 3, ...)
173
3.2 Equations of Kalman filter Kalman filter is an iterative process. The filtering process in one cycle is as follows: One-step state prediction: ˆ k−1 ˆ k,k−1 = Φk,k−1 X (6) X
Where ωk is angular rate; ∆ωk is angular rate increment; k is a certain moment. The equation above shows that the output of the MEMS gyroscope at a certain moment is equal to the angular rate of the previous moment plus an angular rate increment. The difference between static data and dynamic data is whether ∆ωk is equal to 0◦ /s.
One-step prediction mean square error:
The difficulty in modeling for dynamic data is that the value of ∆ωk and how it changes as time goes on are unknown. So this paper assumes that the angular rate increment at k is equal to the angular rate increment at k−1: (2) ∆ωk = ∆ωk−1 .(k = 1, 2, 3, ...)
State update:
Combining equations (1) and (2), the model of MEMS gyroscope output data is obtained as follow: 11 ωk−1 ωk = , (k = 1, 2, 3...) (3) 01 ∆ωk ∆ωk−1 Angular velocity is a state of the object being measured. For a MEMS gyroscope, angular velocity is an input. And how it changes is completely unknown and unpredictable. The above model is a relatively reasonable one for the output data of gyroscope. 3. DESIGN OF KALMAN FILTER The kalman filter algorithm was first proposed by R.E.Kalman in 1960 (Kalman (1960)). Kalman filter is an optimal estimation method that is essentially different from conventional filtering methods. It applies information estimation theory to process the data. This method is available for estimating one or more dimensional random processes which can be either stationary or non-stationary. Because the noise signal of the MEMS gyroscope is random, kalman filter can be used to process the output data to improve its accuracy.
T Pk,k−1 = Φk,k−1 Pk−1 ΦT k,k−1 + Γk,k−1 Qk−1 Γk,k−1
Kalman filter gain: −1 Kk = Pk,k−1 HkT Hk Pk,k−1 HkT + Rk
There is a linear discrete stochastic system, assuming that its system equation is shown in equation (4) and its measurement equation is shown in equation (5), Xk = Φk,k−1 Xk−1 + Γk,k−1 Wk−1 (4) (5) Zk = Hk Xk + Vk Where Xk is the state variable of the system; Φk,k−1 is the state-transition matrix; Wk is the process noise; Γk,k−1 is the noise input matrix; Zk is the measurement vector; Hk is the measurement matrix; Vk is the measurement noise. The process noise Wk and the measurement noise Vk are Gaussian white noise. The covariance of Wk is Q and that of Vk is R. If the linear discrete stochastic system satisfies the above conditions, the state can be estimated by the kalman filter algorithm. 196
(8)
ˆ k,k−1 + Kk Zk − Hk X ˆk = X ˆ k,k−1 X
(9)
Pk = [I − Kk Hk ] Pk,k−1
(10)
Posterior estimation mean square error:
Output update: ˆk Zˆk = Hk X
(11)
ˆ k,k−1 is the priori state estimate which is Where X ˆ k is the predicted according to the previous state; X posterior state estimate which is the correction of the priori estimate by the measurement data Zk ; Pk,k−1 is the priori estimate mean square error; Kk is kalman filter gain which ˆ k is ˆ k,k−1 ; X is used to correct the priori state estimate X ˆ the posterior estimate mean square error; Zk is the output of kalman filter; I is the identity matrix. The process of kalman filter is shown in figure 1. m
)
)
3.1 State Space Equation of Kalman Filter
(7)
*
m
Fig. 1. The Process of Kalman Filter (Xia et al. (2013)) In section 2, a model of the MEMS gyroscope output data has been obtained. Since there is random noise in the measured angular rate, it can be expressed in the form of equations (4) and (5) as follows: 1 1 ωk−1 ωk (12) = + Wk−1 0 1 ∆ωk ∆ωk−1
IFAC E-CoSM 2018 174 Changchun, China, September 20-22, 2018 Shuo Cai et al. / IFAC PapersOnLine 51-31 (2018) 172–176
ω k ∆ ωk
=
10 01
ωk ∆ωk
+ Vk
(13)
The state variables are angular rate and angular rate ωk difference, that is Xk = . The measurement vector ∆ωk ω k is Zk = . The state-transition matrix is Φk,k−1 = ∆ ωk 11 . The noise input matrix is Γk,k−1 = 1. The 01 10 measurement matrix is Hk = . 01 Applying kalman filter to MEMS gyroscopes, Q and R are required. The values of Q and R are set based on 0.0001 0 experience. In this paper, Q is set to , and 0 0.0001 10 R is set to . In order to activate kalman filtering, the 01 ˆ 0 and that of initial value of the posterior state estimate X the posterior estimate P0 are required. mean square error 0.1 0 0.1 ˆ 0 is set to in this , and P0 is set to X 0 0.1 0.1 paper.
Fig. 2. Communication Schematic of Data Acquisition System before and after filtering is shown in figure 3 and figure 4. Limited to space, the figures show only 100 seconds of the first two sets of data. The blue line is the original static data, and the red line is the filtered data. It can be seen from the figure that the drift noise of MEMS gyroscope is reduced. The variance of static data before and after filtering is given in table 1. As can be seen from the table, the variance of static data is reduced by 90% after filtering. So it is proved that the method proposed in this paper has a good performance on the static data.
4. TESTING AND RESULTS
4.1 Data Acquisition System The data acquisition system includes MEMS gyroscope, PC, control chip, power circuit, clock circuit and reset circuit. The control chip used in this paper is STC89C52, and the MEMS gyroscope is MPU6050. The measurement range of MPU6050 is set to ±250◦ /s. MPU6050 passes the measured data to STC89C52RC, and the PC reads the data from STC89C52RC through the serial debugging software XCOM. The communication protocol between STC89C52RC and MPU6050 is IIC protocol, and that between XCOM and STC89C52RC is RS232 protocol. The communication schematic of data acquisition system is shown in figure 2 . 4.2 Static test In the static test, four sets of data were collected when the MPU6050 was stationary, that is the input angular rate of MEMS gyroscope is 0◦ /s. The collection time for each set of data is one hour. There is a constant drift in the collected data which should first be removed. Because the constant drift can not be measured, the average of the four sets of static data is used to estimate the constant drift, and its value is −0.8375◦ /s. Then, it is subtracted from all the collected data. This is also needed when dealing with the dynamic data. Kalman filter is applied to the processed data above according to section 3. The comparison of static data 197
6
Static data Static data after filtering
4 Angular rate ω (°/s)
In order to verify whether the method proposed in this paper can be used to reduce the random noise, a data acquisition system is built to get the experimental data and the static and dynamic tests were carried out.
2 0 −2 −4 −6 0
20
40 60 Time t(s)
80
100
Fig. 3. Comparison of Static Data Before and After Filtering (Set 1) Table 1. Variance of Static Data Before and After Filtering Data Set Set 1 Set 2 Set 3 Set 4
Before Filtering 1.9612 1.9450 1.9604 1.9745
After Filtering 0.1718 0.1687 0.1707 0.1643
4.3 Dynamic Testing In the dynamic test, both the simulative dynamic data and the real dynamic data are used to verify the effectiveness of the proposed method.
IFAC E-CoSM 2018 Changchun, China, September 20-22, 2018 Shuo Cai et al. / IFAC PapersOnLine 51-31 (2018) 172–176
6
2 0 −2 −4 −6 0
Differential data after filtering Sine differential data
0.3 Differential data ∆ω (°/s)
Angular rate ω (°/s)
0.4
Static data Static data after filtering
4
175
0.2 0.1 0 −0.1 −0.2 −0.3
20
40 60 Time t(s)
80
−0.4 0
100
20
40 60 Time t(s)
80
100
Fig. 4. Comparison of Static Data Before and After Filtering (Set 2)
Fig. 6. Comparison of Simulative Dynamic Difference Data Before and After Filtering
Because the actual input angular rate in the dynamic data is unknown, which will make it difficult to judge the filtering effect, a sinusoidal signal is added to the static data in section 4.2 to simulate the dynamic data. The sinusoidal signal simulates the real input angular rate. Kalman filter is applied to the simulative dynamic data, and the parameters are set according to section 3. The filtering performance of simulative dynamic data is shown in figure 5 and figure 6. The mean square error of simulative dynamic data before and after filtering is given in table 2.
filtering, and the filtered data (red line) almost coincides with the simulative real input angular rate (green line). Because the differential data of the simulative dynamic data is too large, it is not given in figure 6. As can be seen, the method also has good performance on differential data. It can be seen from table 2 that the mean square error of four sets of simulative dynamic data is reduced by 90% after filtering. So it proves that the method proposed in this paper has good performance on simulative dynamic data.
40
Simulative Dynamic data Simulative Dynamic data after filtering Sine data
20
300
10
−10 −20 −30 0
Dynamic data Dynamic data after filtering
200
0 Angular rate ω (°/s)
Angular rate ω (°/s)
30
In order to verify the availability of the proposed method to the real dynamic data, four sets of dynamic data are collected, and the proposed filter algorithm is applied to the four sets of data. The filtering effect is shown in figure 7 and figure 8, and the variance of differential data before and after filtering is given in table 3.
20
40 60 Time t(s)
80
100
Fig. 5. Comparison of Simulative Dynamic Data Before and After Filtering
Before Filtering 1.9612 1.9450 1.9604 1.9745
0 −100 −200 −300 0
Table 2. Mean Square Error of Simulative Dynamic Data Before and After Filtering Data Set Set 1 Set 2 Set 3 Set 4
100
5
Time t(s)
10
15
Fig. 7. Comparison of Dynamic Data Before and After Filtering (Set 1)
After Filtering 0.1821 0.1810 0.1819 0.1751
As can be seen in Figure 5, the random noise in the simulative dynamic data is significantly reduced after 198
As can be seen from figure 7 and figure 8, the trend of the filtered data(red line) is consistent with that of the data before filtering(blue line), and the oscillation is significantly reduced. The data in table 3 shows that the variance of dynamic difference data is reduced by about
IFAC E-CoSM 2018 176 Changchun, China, September 20-22, 2018 Shuo Cai et al. / IFAC PapersOnLine 51-31 (2018) 172–176
300
Dynamic data Dynamic data after filtering
Angular rate ω (°/s)
200 100 0 −100 −200 0
5
Time t(s)
10
15
Fig. 8. Comparison of Dynamic Data Before and After Filtering (Set 2) 90% after filtering. So it proves that the proposed method also has good effect on real dynamic data. Table 3. Variance of Dynamic Difference Data Before and After Filtering Data Set Set 1 Set 2 Set 3 Set 4
Before Filtering 616.7533 985.9353 369.3168 658.2255
After Filtering 77.5989 74.7044 34.9843 73.7839
Compared with the AR model in previous studies(Xia et al. (2013), et al.), the model in this paper is not only applicable to static data, but also to dynamic data. The experimental results show that the method in this paper has good filtering effect. 5. CONCLUSION In order to reduce the random noise in the output data of MEMS gyroscope and improve the accuracy of MEMS gyroscope, this paper proposes a direct modeling method to establish the output data model. Different from previous data-driven approaches, the method in this paper is a mechanism modeling method. Therefore, the model is suitable for both static data and dynamic data of MEMS gyroscope. Based on this model, the kalman filter is designed to process the output data. Then the static data test and the dynamic data test are carried on. The experimental results show that the proposed method has good performance on both static data and dynamic data. REFERENCES Bai, J., Chen, W., and Cai, T. (2013). Compensation for mems gyroscope zero bias stability. In Chinese Automation Congress (CAC), 744–748. Cao, H., Lv, H., and Sun, Q. (2015). Model design based on mems gyroscope random error. In IEEE International Conference on Information and Automation, 2176–2181. Chen, W.C., Gao, G.W., Wang, J., Liu, L.L., and Li, X.L. (2012). The study of the mems gyro zero drift signal based on the adaptive kalman filter. In International Conference on Remote Sensing, 635–639. 199
Jie, L.I. and Zhang, W.D. (2006). Research on the application of the time-serial analysis based kalman filter in mems gyroscope random drift compensation. Chinese Journal of Sensors & Actuators, 19, 2215–2219. Kalman, R.E. (1960). A new approach to linear filtering and prediction problems. Journal of basic Engineering, 82(1), 35–45. Li, Z., Duan, F., and Ma, J. (2012). Random drift compensation of mems gyroscope based on support vector machine. Chinese Journal of Sensors & Actuators, 25(8), 1084–1087. Ruan, X.G. and Y. U., M.M. (2014). Modeling research of mems gyro drift based on kalman filter. In Chinese Control and Decision Conference, 2949–2952. Shangguan, Y., Chen, J., Song, C., and Han, Y. (2015). Research on the compensation in mems gyroscope random drift based on time-series analysis and kalman filtering. In Chinese Control Conference, 2078–2082. Xia, Y., Chen, W., and Peng, W. (2013). Research on random drift modeling and a kalman filter based on the differential signal of mems gyroscope. In Chinese Control and Decision Conference, 3233–3237. Zhu, W.J., Wang, G.L., Qiao, Z.T., and Gao, F.Q. (2014). A novel noise reduction algorithm of mems gyroscope based on compressive sensing and lifting wavelet transform. Key Engineering Materials, 609610(609-610), 1138–1143.