sensors
Article
Research on Filtering Algorithm of MEMS Gyroscope
Based on Information Fusion
Hui Guo
and Huajie Hong *
College of Intelligence Science and Technology, National University of Defense Technology,
Changsha 410073, China
* Correspondence: honghuajie@nudt.edu.cn
Received: 2 July 2019; Accepted: 13 August 2019; Published: 15 August 2019
Abstract: As an important inertial sensor, the gyroscope is mainly used to measure angular velocity
in inertial space. However, due to the influence of semiconductor thermal noise and electromagnetic
interference, the output of the gyroscope has a certain random noise and drift, which affects the
accuracy of the detected angular velocity signal, thus interfering with the accuracy of the stability of
the whole system. In order to reduce the noise and compensate for the drift of the MEMS (Micro
Electromechanical System) gyroscope during usage, this paper proposes a Kalman filtering method
based on information fusion, which uses the MEMS gyroscope and line accelerometer signals to
implement the filtering function under the Kalman algorithm. The experimental results show that
compared with the commonly used filtering methods, this method allows significant reduction of the
noise of the gyroscope signal and accurate estimation of the drift of the gyroscope signal, and thus
improves the control performance of the system and the stability accuracy.
Keywords: MEMS gyroscope; line accelerometer; noise; drift; Kalman filter
1. Introduction
A photoelectric stability platform is a device that can effectively isolate the carrier disturbance
and keep the visual axis stable. As an important inertial sensor of photoelectric stability platform,
the gyroscope is mainly used for the angular velocity of the sensitive inertia space of each axis.
The measured angular velocity is fed back to the servo control system to form a speed closed loop,
thereby isolating the external disturbance and ensuring the stability of the platform. The MEMS
gyroscope is widely used in photoelectric stabilization systems due to its small size and low cost.
However, due to the influence of devices and environmental interference, the output of the MEMS
gyroscope has a certain noise and drift, which reduces the accuracy of the signal and affects the stability
of the system.
At present, there are many methods for processing gyroscope signal noise, which can be roughly
divided into two categories: one is the based on the model compensation method, that is, modeling the
random noise of the gyroscope and offsetting it in accordance with the model, e.g., the Kalman filter
method [1–3], statistical filtering, etc.; the other is the direct filtering method, which directly filters the
output signal of the gyroscope, e.g., low-pass filtering, Wavelet filtering [4–7], adaptive filtering [8],
and so on.
In the literature [9], based on the time series model of gyroscope zero drift data, the author uses
the Kalman algorithm to process the drift data. The simulation results show that the method can
effectively filter the static gyroscope signal, but the dynamic signal filtering effect is poor. Another
paper [10] proposes an analytical method to estimate the measurement noise variance in Kalman filter
and apply it to the Kalman filter in real time; experiments show that this method could carry out a more
accurate result in estimating the measurement noise variance and improve the accuracy of Kalman
Sensors 2019, 19, 3552; doi:10.3390/s19163552
www.mdpi.com/journal/sensors
Sensors 2019, 19, 3552
2 of 20
filter; however, it is not able to estimate the drift of the sensor. In the literature [11], Luming Li combines
Kalman filtering with neural networks to estimate the variance of the Kalman filter measurement
noise through neural networks, thus solving the problem of inaccurate selection of measurement noise
variance. Experiments show that the algorithm better suppresses the random noise of the gyroscope.
However, since this method is also based on time series modeling, the filtering effect on dynamic
signals is unsatisfactory. In the literature [12], the Kalman filter is used to fuse the two sensor signals
of the MEMS gyroscope and the accelerometer to suppress the gyroscope random noise, meanwhile
estimating the drift of the accelerometer. However, because the existence of the constant drift of the
gyroscope is not taken into consideration in this algorithm, there is a certain error with the actual
situation. In the literature [8], in order to eliminate the random noise of the fiber optic gyroscope,
the digital low-pass filtering, wavelet filtering, adaptive filtering, and variable-step adaptive filtering
methods are compared for filtering. The comparison shows that the variable step size adaptive filtering
method has the best filtering effect on the gyroscope random noise. However, this method cannot
simultaneously estimate the constant drift of the sensor.
For the estimation of gyroscope signals drift, the literature [13] uses six accelerometers placed
symmetrically on the UAV’s rotary axis to calculate the angular velocity. The Kalman filter algorithm
is used to fuse the angular velocity of the gyroscope output with the angular velocity calculated by
the accelerometer to estimate the constant drift of the gyroscope. The simulation results show that
the method can effectively compensate the constant drift of the gyroscope and has certain application
value. However, this method has only a certain effect in the simulation and has not been verified in
the experiment.
Although the above researches have achieved certain results on the filtering and drift estimation of
the gyroscope signal, the filtering and drift estimation of the gyroscope noise often cannot be obtained
at the same time. Although some scholars also use gyroscopes and line accelerometers to achieve gyro
filtering and drift estimation, they ignore the existence of linear accelerometer drift, making the results
less accurate. Based on the summary of the current research results, this paper uses the Kalman filter
algorithm that combines the gyroscope signal and the accelerometer signal to process the gyroscope
signal and also estimate the accelerometer signal filtering and drift, thereby improving the detection
accuracy of the sensor.
2. Analysis of MEMS Gyroscope Error
2.1. Analysis of Noise Sources of MEMS Gyroscopes
The noise of MEMS gyroscopes is mainly caused by imperfect structure and environmental
interference. In order to identify and characterize the noise source that causes the gyroscope noise, the
Allan variance method is used to analyze the MEMS gyroscope signal. This method is characterized by
the ability to characterize and identify the statistical properties of the entire noise, meanwhile analyzing
the source of the identified noise term [8].
Let the length of the gyroscope signal be L and the sampling frequency be f, then the sampling
period is t0 = 1/ f . The average gyroscope signal is divided into K groups (K ≥ 2), then the data length
of each group is N = L/K and the length of each group of data is t = t0 ∗ N, which is also called the
correlation time [14].
Calculate the average value ωk (N ) of each group at each correlation time t, then
ωk (N ) =
N
1 X
ω(k−1)∗N+i K = 1, 2, · · · , K
N
i=1
(1)
Sensors 2019, 19, 3552
3 of 20
Then the Allan method at the relevant time t is defined as
σ2A (t) =
K−1
Xh
i2
1
ω j+1 (N ) − ω j (N )
2(K − 1)
(2)
j=1
Sensors 2019, 19, x FOR PEER REVIEW
3 of 21
The Allan variance curve can be obtained by taking different correlation times t and calculating
The Allan variance
canσ2be(tobtained
by taking
correlation times t and calculating
). The square
the corresponding
Allan curve
variance
root σdifferent
A (t) of the Allan variance is called the Allan
A
The square root 𝜎 (t) of the Allan variance is called the
the
corresponding
variance
standard
deviation Allan
and the
curve of𝜎 σ(t).
A (t) ∼ t in the double logarithmic coordinate system is called
Allan
standard
deviation
and
the
curve
of
𝜎 (t)~𝑡
in [15].
the double logarithmic coordinate system is
the Allan standard deviation double logarithmic
curve
calledThe
therandom
Allan standard
logarithmic
curve [15].
drift errordeviation
of MEMSdouble
gyroscope
mainly includes
five noise sources such as quantization
driftangular
error of
MEMS
gyroscope
mainly
includes
noise
sources
such
as
noise,The
raterandom
ramp noise,
rate
random
walk noise,
angular
randomfive
walk
noise,
and zero
offset
quantization
noise,
rate
ramp
noise,
angular
rate
random
walk
noise,
angular
random
walk
noise,
instability noise [16]. Since these noise sources are reflected in different correlation time regions,
and
zero offset
instability
Since
these noise
sources
reflected in
different
correlation
the Allan
variance
methodnoise
can [16].
analyze
different
noises
of theare
gyroscope.
The
Allan variance
of
2
2
2
time
regions,
the
Allan
variance
method
can
analyze
different
noises
of
the
gyroscope.
The
Allan
the quantization noise is σQ (t) = 3Q /t . The Allan variance of the angular random walk noise is
variance
of2 the
noise isnoise
𝜎 (t)of=the
3𝑄 zero-bias
/𝑡 . The Allan
variance
random
walk
(t)the
σ2N (t) = N
/t. quantization
The Allan variance
instability
is σ2Bof
= angular
2B2 ln2/π.
The Allan
noise is 𝜎of (t)
𝑁 /𝑡. The
variance
noiseisofσ2the
𝜎 (t) =of2𝐵
The
(t)zero-bias
variance
the=angular
rateAllan
random
walk noise
= K2 t/3.instability
The Allan is
variance
the𝑙𝑛2/𝜋.
rate ramp
K
(t)
Allan
variance
of
the
angular
rate
random
walk
noise
is
𝜎
=
𝐾
𝑡/3.
The
Allan
variance
of
the
2
2
2
noise is σR (t) = R t /2. Among them, Q, N, B, K, and R are various error coefficients.
rate ramp
is gyroscope
𝜎 (t) = 𝑅 contains
𝑡 /2. Among
𝑄, N,sources
B, K, and
are
various error
coefficients.
If the noise
MEMS
thesethem,
five noise
andR is
statistically
independent.
Then
If
the
MEMS
gyroscope
contains
these
five
noise
sources
and
is
statistically
independent.
the Allan variance can be decomposed into the sum of the Allan variances of the noise sources. Then
the Allan variance can be decomposed into the sum of the Allan variances of the noise sources.
2
2
) ++σ2N𝜎(t(t)
) ++σ𝜎
) ++σ𝜎
) ++σ𝜎2R (t(t)
)
σ2A𝜎(t)(t)
==
σ2Q𝜎(t(t)
(3)
B (t(t)
K (t(t)
(3)
2
X
2
2
2 2
3Q2
N𝑁2
2B
i
2𝐵 ln2
𝑅 t𝑡 =
𝑙𝑛2+ K𝐾 t𝑡+ R
σ2A𝜎(t)(t)
= = 3𝑄
+
ϕ
+
+ 3 + 2 =
𝜑i t𝑡
t2𝑡 + t 𝑡 + π𝜋
3
2
i=−2
(4)
(4)
Using the
the least
least squares
squares method,
method, 𝜑
ϕi can
error coefficients
coefficients are
are obtained.
obtained.
Using
can be
be obtained
obtained and
and various
various error
The
The relationship
relationship between
between each
each error
error coefficient
coefficient and
and ϕ𝜑i isis
√
√
√
p
p
ϕ𝜑−2
ϕ
𝜑−1
𝜑ϕ0𝜋π
Q𝑄==
=
,
B
=
=60
60 3𝜑
3ϕ1, ,𝑅R==3600
3600 2𝜑
2ϕ2
√ ,N
, 𝑁 = 60 𝐵 = 2ln2 ,,𝐾K =
60
2𝑙𝑛2
3600
3
3600√3
In
this paper,
paper,the
thestatic
staticdata
data
MEMS
gyroscope
is collected
at sampling
the sampling
frequency
of 1
In this
ofof
MEMS
gyroscope
is collected
at the
frequency
of 1 KHz.
KHz.analysis
The analysis
are shown
in Figure
1.
The
resultsresults
are shown
in Figure
1.
Figure
Allan variance
variance logarithmic
logarithmic curve.
curve.
Figure 1.
1. Allan
The error coefficients obtained by Allan analysis of variance are
𝑄 = 5.4412
10 , 𝑁 = 8.0041
10 , 𝐵 = 0.0258, K = 3.973, 𝑅 = 121.3356
As can be seen from the above figure, the noise composition of the MEMS gyroscope is mainly
composed of quantization noise and angular random walk noise. Therefore, the filtering of MEMS
gyroscope noise is mainly to filter the two noise sources.
𝜙 (ω) = |𝐺 (𝑗ω)| ϕ (ω)
(6)
According to Equation (6), the system mean square error caused by the gyroscope error is
Sensors 2019, 19, 3552
|𝐺 (𝑗ω)| ϕ (ω) 𝑑𝜔
𝜀 =
4 of(7)
20
It can be seen from Equation (7) that the influence of the gyroscope error on the output of the
Theiserror
coefficients
obtained
Allan density
analysisof
ofthe
variance
are error and its transfer function
system
determined
by the
powerby
spectral
gyroscope
in the control system. When the transfer function of the control system is fixed, the larger the noise
−7
−4
Q gyroscope,
= 5.4412 × 10
, N = 8.0041
× 10square
, B =error
0.0258,
K =system
3.973, R
= 121.3356
and drift of the
the larger
the mean
of the
output
due to it.
In addition, in the platform stability control loop, due to the presence of gyro noise, the
As can be seen from the above figure, the noise composition of the MEMS gyroscope is mainly
controller parameters are often not further optimized, which limits the improvement of system
composed of quantization noise and angular random walk noise. Therefore, the filtering of MEMS
control performance. The existence of constant drift of the gyroscope will cause the system to have
gyroscope noise is mainly to filter the two noise sources.
a smaller amplitude control output even when the platform is stable, which causes the platform to
rotate
slowly,ofaffecting
its stability
accuracy,
moreSystem
serious may cause the system to not work
2.2.
Influence
MEMS Gyroscope
Error
on Stableand
Platform
properly.
The
influence
the error
of the the
MEMS
gyroscope
on the stable
platform
canand
be
Therefore,
in of
order
to reduce
influence
of MEMS
gyroscope
errorcontrol
on thesystem
system
expressed
by
the
mean
square
error
of
the
system
output
under
the
influence
of
the
noise
and
drift
of
improve the control performance of the system, it is necessary to process the noise and constant
the
The system control model is shown in Figure 2.
driftgyroscope.
of the gyroscope.
Figure 2. Schematic diagram of the control system structure.
Figure 2. Schematic diagram of the control system structure.
In Figure 2, C(s) is the controller of the stable loop, G(s) is the transfer function of the system
3. Design and Simulation of the Filter Algorithm
model, H(s) is the transfer function of the sensor, r(t) is the speed command entered by the system, v(t)
is theThere
speedare
of many
the system
output,
and n(t)
is the error of
the as
gyroscope.
filtering
methods
for gyroscopes,
such
low-pass filter, Kalman filter, forward
the Figure
2, the
transfer function
of the gyroscope
error input
to the system
is
linearAccording
filter, andtowavelet
filter.
Considering
the feasibility
and real-time
requirements,
theoutput
Kalman
filter algorithm and forward linear prediction filter algorithm will be highlighted below. We
(s)Gthe
(s)best filter algorithm.
H (to
s)C
compare the filtering effects of the two
find
(5)
Gc (algorithms
s) = −
1 + H (s)C(s)G(s)
From the linear system theory, the output power spectral density of a linear system is equal to the
product of the input power spectral density and the system power transfer function [17]. According to
the system shown in Figure 2, if the power spectral density of the gyro error is φr (ω). The transfer
function of the gyroscope noise input to the system output is Gc (s). The power spectral density of the
system output caused by the gyro noise input is
2
φv (ω) = Gc ( jω) φr (ω)
(6)
According to Equation (6), the system mean square error caused by the gyroscope error is
Z
∞
2
ε =
−∞
2
Gc ( jω) φr (ω)dω
(7)
It can be seen from Equation (7) that the influence of the gyroscope error on the output of the
system is determined by the power spectral density of the gyroscope error and its transfer function in
the control system. When the transfer function of the control system is fixed, the larger the noise and
drift of the gyroscope, the larger the mean square error of the system output due to it.
In addition, in the platform stability control loop, due to the presence of gyro noise, the controller
parameters are often not further optimized, which limits the improvement of system control
performance. The existence of constant drift of the gyroscope will cause the system to have a smaller
Sensors 2019, 19, 3552
5 of 20
amplitude control output even when the platform is stable, which causes the platform to rotate slowly,
affecting its stability accuracy, and more serious may cause the system to not work properly.
Therefore, in order to reduce the influence of MEMS gyroscope error on the system and improve
the control performance of the system, it is necessary to process the noise and constant drift of
the gyroscope.
3. Design and Simulation of the Filter Algorithm
There are many filtering methods for gyroscopes, such as low-pass filter, Kalman filter, forward
linear filter, and wavelet filter. Considering the feasibility and real-time requirements, the Kalman
filter algorithm and forward linear prediction filter algorithm will be highlighted below. We compare
the filtering effects of the two algorithms to find the best filter algorithm.
3.1. Improved Kalman Filter Algorithm
3.1.1. Design of Kalman Filtering Algorithm Based on Information Fusion
Kalman filter is a filtering method based on the minimum mean square error criterion. It obtains
an estimate of the state value through an iterative calculation. And it is the best estimate for linear
systems with Gaussian white noise. Due to the high accuracy and good real-time performance of
Kalman filter, it has been widely used in signal processing.
The equation of state for the discrete time process of the Kalman filter is expressed as
xk = Axk−1 + Buk−1 + Fωk−1
(8)
The measurement equation is expressed as
yk = Cxk + Dvk
(9)
In Equation (9), xk is the system state quantity at time k, uk−1 is the system control quantity at
time k − 1, A is the state transition matrix, B is the control matrix, C is the measurement matrix, and D
and F are the noise matrix. ωk is the system process noise, vk is the system measurement noise, and ωk
and vk are mutually independent, normally distributed white noise.
ωk ∼ N (0, Q)
(10)
vk ∼ N (0, R)
(11)
Q and R are the covariance matrices of system noise and measurement noise, respectively.
The steps of the Kalman filter algorithm are as follows.
State one-step prediction:
xk|k−1 = Axk−1 + Buk−1
(12)
One-step prediction of covariance matrix:
Pk|k−1 = APk−1 AT + Q
(13)
K = Pk|k−1 CT CPk|k−1 CT + R
(14)
xk = xk|k−1 + K yk − Cxk|k−1
(15)
Pk = (I − KC)Pk|k−1
(16)
Calculate the filter gain:
State estimation:
Covariance matrix estimation:
Sensors 2019, 19, x FOR PEER REVIEW
6 of 21
information, and∆𝑎 is the angular acceleration drift of the sensor. Therefore, the system equations
6 of 20
and observation equations after the amplification state are
Sensors 2019, 19, 3552
𝑿 |
= 𝑨𝑿
+ 𝑩𝑾
(19)
According to the motion relationship, 𝒀when
the+angular
velocity value vk and the angular
= 𝑪𝑿
𝑽
acceleration value ak of the system at time k are known, the angle and angular velocity of the system at
where
time
k + 1 can be obtained as follows
⎡1
⎢0
𝑨 = ⎢0
⎢0
⎣0
𝑇
1
0
0
0
1 0 0 0 0
− ⎤
1
⎡
1 0 0 0 0
1 0 ∗ a0 ∗0t⎤2
x0k+𝑇1 =−𝑇xk ⎥+ vk⎢∗0 t +
k ⎥
0
0
1 0 0⎥,𝑪 = 0 1 0 0 0
,𝑩
=
2
⎥
⎢
1 0 0
−𝑇
0
0
1
0
0
⎢0
⎥
0
0
1
0⎥
v1k+1⎦ = v⎣k0+0ak 0∗ t 0 1⎦
0
0
0
1
(17)
0
(18)
where
T is the sampling
time. (17) and (18), it is possible to construct a state equation for Kalman
According
to the Formulae
Theusing
angleinformation
information
canas be
solved
by velocity,
the gravity
component
to whichThis
the avoids
linear
filtering
such
angle,
angular
and angular
acceleration.
accelerometer
is
sensitive.
The
angular
velocity
information
is
measured
by
a
gyroscope.
the use of ARMA models or state equations of the original system that are difficult to obtain The
for
angular filtering.
acceleration information is measured by a linear accelerometer. It can be shown in Figure 3.
Kalman
In Figure
two
linear accelerometers
are placedhave
symmetrically
theorder
axis of
is
Since
both3,the
gyroscope
and the accelerometer
noise and about
drift, in
to rotation.
improve G
the
the
acceleration
of
gravity;
𝑎
and
𝑎
are
the
output
values
of
the
two
linear
accelerometers,
algorithm’s effect on sensor noise
reduction
and estimation of sensor drift, the state of the Kalman
1
2
h of the line accelerometer
iT
respectively;
d
is
the
distance
from the
rotation;𝜃
is the angle
at
filter is augmented as X = x v
. Where
x isaxis
the of
angle
information,
v is the
∆v a ∆a
which
the
platform
rotates;
and𝛼
is
the
angular
acceleration
of
the
platform
rotation.
According
to
angular velocity information, ∆v is the angular velocity drift of the sensor, a is the angular acceleration
the relationship,
youiscan
information,
and ∆a
the get
angular acceleration drift of the sensor. Therefore, the system equations and
observation equations after the amplification
= 𝛼𝑑are
− 𝐺𝑐𝑜𝑠𝜃
𝑎1state
(
The above formula is combined
Xk|k−1
BWk−1
k−1−+
𝑎2 =
= AX
−𝛼𝑑
𝐺𝑐𝑜𝑠𝜃
Yk = CXk + Vk
(20)
(21)
(19)

 =𝑎 −𝑎

2 
2
𝛼
(22)
 1 T −T T2 − T2 
 1 0 2 0 0 0 






 0 1 0 0 0 
 0 1 0
 1 0 0 0 0 
T −T 






𝑎1 +0𝑎 0 , C =  0 1 0 0 0 
 0 (−
A =  0 0 1
0
0
0 ,𝜃B==arccos

(23)
) 







 0 0 0 2𝐺1 0 
 0 0 0
0 0 0 1 0
1
0 









Thus the angle
of the
0 0platform
0 0 can
1 be obtained.
0 and
0 angular
0
0 acceleration
1
It can be deduced from the above that the improved Kalman filter algorithm can not only filter
where T is the sampling time.
the gyroscope signal and estimate its constant value drift, but also filter the acceleration signal and
The angle information can be solved by the gravity component to which the linear accelerometer
estimate the constant value drift of the accelerometer. The processing of the acceleration signal is to
is sensitive. The angular velocity information is measured by a gyroscope. The angular acceleration
better achieve the algorithm’s filtering of the gyroscope signal and the estimation of the gyroscope
information is measured by a linear accelerometer. It can be shown in Figure 3.
drift.
where
θ
α
Figure 3. Schematic diagram of the installation of the linear accelerometer.
Figure 3. Schematic diagram of the installation of the linear accelerometer.
In Figure 3, two linear accelerometers are placed symmetrically about the axis of rotation. G is the
3.1.2. Proof of
Kalman Filter Algorithm Based on Information Fusion
acceleration
of Stability
gravity; aof
1 and a2 are the output values of the two linear accelerometers, respectively; d
is theThe
distance
of the
line accelerometer
from
theisaxis
of rotation;
θ is the angle
which the
platform
stability
problem
of the Kalman
filter
related
to its application
in at
practical
engineering.
rotates;
andthe
α isstability
the angular
acceleration
of be
theverified
platformafter
rotation.
According
to theThe
relationship,
you
Therefore,
of the
filter should
the filter
is designed.
filter stability
can
get
problem requires studying the influence of the initial value of the parameters on the filter stability.
a1 = αd − Gcosθ
(20)
Sensors 2019, 19, 3552
7 of 20
a2 = −αd − Gcosθ
The above formula is combined
(21)
a1 − a2
2
a + a2
θ = arccos(− 1
)
2G
α=
(22)
(23)
Thus the angle and angular acceleration of the platform can be obtained.
It can be deduced from the above that the improved Kalman filter algorithm can not only filter
the gyroscope signal and estimate its constant value drift, but also filter the acceleration signal and
estimate the constant value drift of the accelerometer. The processing of the acceleration signal is to
better achieve the algorithm’s filtering of the gyroscope signal and the estimation of the gyroscope drift.
3.1.2. Proof of Stability of Kalman Filter Algorithm Based on Information Fusion
The stability problem of the Kalman filter is related to its application in practical engineering.
Therefore, the stability of the filter should be verified after the filter is designed. The filter stability
problem requires studying the influence of the initial value of the parameters on the filter stability.
That is, as the filtering time increases, the estimated value of the state and the error variance matrix are
not affected by the selected initial value [18]. To prove the stability of the filter, the following theorem
is used:
Filter stability theorem: If the system is fully controllable and fully observable, the Kalman filter
is uniformly progressively stable [19,20].
Among them, the observability and controllability criteria of linear systems are as follows.
(1) Linear system controllability criteria: For linear constant discrete system
x(k + 1) = Ax(k) + Bu(k)
h
B
The necessary andisufficient condition for complete controllability is that the rank of the matrix
AB · · · An−1 B is n.
(2) Linear System Observability Criterion: For linear constant discrete system
(
h
(24)
x(k + 1) = Ax(k) + Bu(k)
y(k) = Cx(k) + v(k)
(25)
The necessary and sufficient condition for complete observability is that the rank of the matrix
iT
is n. Where n is the dimension of the state vector.
C CA · · · CAn−1
According to Equation (17),
h
rank B
AB
A2 B
A3 B
A4 B
i
h
rank C
CA
CA2
CA3
CA4
iT
=5
=5
(26)
(27)
As can be seen from the above equation, the Kalman filtering algorithm proposed in this paper is
completely controllable and fully observable. According to the filter stability theorem, the algorithm
is stable.
3.2. Forward Linear Prediction Filter Algorithm
The principle of the forward linear predictive filter algorithm is to multiply the previous gyroscope
signal by the corresponding weight to predict the gyroscope signal at the current time. The algorithm
begins with an initial value and then calculates the difference between the current gyroscope actual
Sensors 2019, 19, 3552
8 of 20
value and the predicted value. The weight vector is adjusted according to the minimum mean square
error algorithm to converge to a stable weight [21].
x̂(n) =
K
X
ai x(n − i) = AT X(n − 1)
(28)
i=1
X(n − 1) = [x(n − 1) x(n − 2) · · · x(n − K)]T is the vector of the gyro signals from the first K
moments; ai is the weight and K is the order of the filter.
The difference between the current gyroscope actual value and the current gyroscope predicted
value and the cost function are
e(n) = x(n) − x̂(n)
(29)
J ( n ) = E e2 ( n )
(30)
According to the theory of minimum mean square error, J (n) should take the minimum value if
the best weight is to be obtained. Therefore, the weight adjustment equation is
A(n + 1) = A(n) + µE[e(n)X(n − 1)]
(31)
In the actual calculation, in order to reduce the amount of calculation, the above equation is
simplified as
A(n + 1) = A(n) + µe(n)X(n − 1)
(32)
In the equation, µ is the step factor, which is generally a smaller value greater than zero [22].
8 of 21
Sensors 2019, 19, x FOR PEER REVIEW
3.3. Simulation Analysis of Filter Algorithm
3.3. Simulation Analysis of Filter Algorithm
For the filtering algorithm designed above, the closed-loop control system shown in Figure 4 is
For the filtering algorithm designed above, the closed-loop control system shown in Figure 4 is
built for simulation analysis. Where r(t) is the angular velocity command signal, G(s) is the transfer
built for simulation analysis. Where r(t) is the angular velocity command signal, G(s) is the transfer
function of the platform system, and C(s) is the controller of the system speed closed loop. The angular
function of the platform system, and C(s) is the controller of the system speed closed loop. The
velocity noise signal vn(t) added by the system is a white noise signal with a variance of 1 (deg/s)2
angular velocity noise signal vn(t) added by the system is a white noise signal with a variance of 1
and an average of 0.5 deg/s. The added acceleration noise signal an(t) is a white noise signal with
(deg/s) and an average2of 0.5 deg/s. The added acceleration
2 noise signal an(t) is a white noise signal
2 and an average value of 1 deg/s2 .
awith
variance
of 5 deg/s
a variance
of 5 (deg/s
) and an average value of 1 (deg/s ) .
Figure 4. Simulation structure of the filtering algorithm.
Figure 4. Simulation structure of the filtering algorithm.
3.3.1. Analysis of Static Filtering
3.3.1. Analysis of Static Filtering
Set the angular velocity command to zero. The acquisition of the gyroscope raw signal, the Kalman
Setsignal,
the angular
to zero.
The
of the 5–9.
gyroscope raw signal, the
filtered
and thevelocity
forwardcommand
linear filtered
signal
areacquisition
shown in Figures
Kalman filtered signal, and the forward linear filtered signal are shown in Figure 5-9.
Figure 4. Simulation structure of the filtering algorithm.
3.3.1. Analysis of Static Filtering
Set the angular velocity command to zero. The acquisition of the gyroscope raw signal, the
Kalman
filtered
Sensors 2019,
19, 3552signal, and the forward linear filtered signal are shown in Figure 5-9.
9 of 20
Sensors 2019, 19, x FOR PEER REVIEW
9 of 21
Sensors 2019, 19, x FOR PEER REVIEW
9 of 21
Figure 5. The angular velocity signal after filtering by the Kalman algorithm.
Figure 5. The angular velocity signal after filtering by the Kalman algorithm.
Figure 6. Estimation of gyroscope drift.
Figure 6. Estimation of gyroscope drift.
Figure 6. Estimation of gyroscope drift.
Figure 7. The angular acceleration signal filtered by the Kalman algorithm.
Figure 7. The angular acceleration signal filtered by the Kalman algorithm.
Figure 7. The angular acceleration signal filtered by the Kalman algorithm.
Sensors 2019, 19, 3552Figure 7. The angular acceleration signal filtered by the Kalman algorithm.
Sensors 2019, 19, x FOR PEER REVIEW
Figure 8. Estimation of accelerometer drift.
Figure 8. Estimation of accelerometer drift.
10 of 20
10 of 21
Figure 9.
9. The
Theangular
angularvelocity
velocitysignal
signalfiltered
filteredby
bythe
the forward
forward linear
linear algorithm.
algorithm.
Figure
can be
be seen
seen from
5–9that
thatthe
theKalman
Kalmanfilter
filteralgorithm
algorithmand
and the
the forward
forward linear
linear filter
filter
ItIt can
from Figures
Figure 5-9
algorithm
have
the
effect
of
reducing
the
gyroscope
noise
level
when
the
gyroscope
noise
level
is
algorithm have the effect of reducing the gyroscope noise level when the gyroscope noise level is
2
(
)
constant.
Among
them,
the
Kalman
filter
algorithm
reduces
the
variance
of
the
noise
from
1
deg/s
constant. Among 2them, the Kalman filter algorithm reduces the variance of the noise from 1
(deg/s
to 0.0056to
. The forward
linear filtering
algorithm
reduces
the variance
of theofnoise
from
(deg/s)
0.0056) (deg/s)
. 2The forward
linear filtering
algorithm
reduces
the variance
the noise
2
(
)
(
)
1
to
0.0119
.
It
can
be
seen
that
the
Kalman
filter
effect
is
better
than
the
forward
linear
deg/s
deg/s
from 1 (deg/s) to 0.0119 (deg/s) . It can be seen that the Kalman filter effect is better than the
filter effect.
Moreover,
the Kalman
filterthe
algorithm
estimate
the constant
drift ofthe
theconstant
gyroscope
more
forward
linear
filter effect.
Moreover,
Kalmancan
filter
algorithm
can estimate
drift
of
accurately.
In addition,
the Kalman
filter algorithm
can simultaneously
the angular acceleration
the
gyroscope
more accurately.
In addition,
the Kalman
filter algorithm filter
can simultaneously
filter the
signal and
estimate the
constant
drift of the
angular
acceleration
signal
and estimate
the accelerometer.
constant drift of the accelerometer.
The
power
spectrum
is
estimated
for
angular
velocity
signals
before
filtering.
The power spectrum is estimated for thethe
angular
velocity
signals
before
and and
after after
filtering.
The
The
result
is
shown
in
Figure
10.
result is shown in Figure 10.
As showed from the Figure 10, from the perspective of the frequency domain, the noise after
forward linear filtering is 15 dB lower than the power spectrum of the original noise. The noise after
the Kalman filtering is reduced by up to 25 dB compared to the power spectrum of the original noise.
Therefore, the Kalman filter algorithm can better reduce the gyroscope noise.
from 1 (deg/s) to 0.0119 (deg/s) . It can be seen that the Kalman filter effect is better than the
forward linear filter effect. Moreover, the Kalman filter algorithm can estimate the constant drift of
the gyroscope more accurately. In addition, the Kalman filter algorithm can simultaneously filter the
angular acceleration signal and estimate the constant drift of the accelerometer.
The power spectrum is estimated for the angular velocity signals before and after filtering. The
Sensors
2019,is19,
3552 in Figure 10.
11 of 20
result
shown
Figure 10. Power spectrum of the angular velocity signal before and after filtering.
Figure 10. Power spectrum of the angular velocity signal before and after filtering.
Sensors 2019, 19, x FOR PEER REVIEW
3.3.2.
Analysis
Dynamic
Filtering
Sensors
2019, 19,of
x FOR
PEER REVIEW
11 of 21
11 of 21
As showed from the Figure 10, from the perspective of the frequency domain, the noise after
sine
angular
speed
command
signal
with
amplitude
2 deg/s
and frequency
Hz. filtered
The
SetSet
thethe
sine
angular
speed
signal
amplitude
2 deg/s
and
frequency
Hz.1noise
The
forward
linear
filtering
is speed
15 command
dB command
lower than
thewith
power
of2the
original
noise. 1The
Setresult
the
sine
angular
signal
withspectrum
amplitude
deg/s
and frequency
1 Hz. after
The
filtered
is
shown
in
Figure
11-12.
result
shownfiltering
in Figures
11 and by
12.up to 25 dB compared to the power spectrum of the original noise.
theisKalman
is reduced
filtered result is shown in Figure 11-12.
Therefore, the Kalman filter algorithm can better reduce the gyroscope noise.
3.3.2. Analysis of Dynamic Filtering
Figure 11. The angular velocity signal filtered by the Kalman algorithm under dynamic commands.
Figure
11.11.
The
angular
Kalmanalgorithm
algorithmunder
underdynamic
dynamic
commands.
Figure
The
angularvelocity
velocitysignal
signalfiltered
filtered by
by the
the Kalman
commands.
Figure
12. The angular velocity signal filtered by the forward linear algorithm under dynamic command.
Figure 12. The angular velocity signal filtered by the forward linear algorithm under dynamic
Figure 12. The angular velocity signal filtered by the forward linear algorithm under dynamic
command.
It can be seen from Figures 11 and 12 that both the Kalman filter algorithm and the forward linear
command.
filter algorithm
can reduce the noise level in the case of dynamic commands. Among them, the Kalman
It can be seen from Figure 11-12 that both the Kalman filter algorithm and the forward linear
can be seen
Figure
thecase
Kalman
filter algorithm
and the
forward
linear
filter Italgorithm
canfrom
reduce
the 11-12
noise that
levelboth
in the
of dynamic
commands.
Among
them,
the
filter algorithm
can reduce
the the
noise
level in
case
of 1dynamic
Among
the
Kalman
filter algorithm
reduces
variance
of the
noise
from
(deg/s) commands.
to 0.02 (deg/s)
. Thethem,
forward
Kalman
filter
algorithm
reduces
the
variance
of
noise
from
1
(deg/s)
to
0.02
(deg/s)
.
The
forward
linear filtering algorithm reduces the variance of the noise from 1 (deg/s) to 0.093 (deg/s) .
linear filtering
algorithm
reduces
variance
of thealgorithm
noise from
1 (deg/s) better
to 0.093
(deg/s)
Therefore,
the filtering
effect
of the the
improved
Kalman
is significantly
than
that of.
Therefore,
the
filtering
effect
of
the
improved
Kalman
algorithm
is
significantly
better
than
that in
of
the forward linear algorithm. Moreover, the convergence process of the Kalman filter algorithm
Sensors 2019, 19, 3552
12 of 20
filter algorithm reduces the variance of noise from 1 (deg/s)2 to 0.02 (deg/s)2 . The forward linear
filtering algorithm reduces the variance of the noise from 1 (deg/s)2 to 0.093 (deg/s)2 . Therefore,
the filtering effect of the improved Kalman algorithm is significantly better than that of the forward
linear algorithm. Moreover, the convergence process of the Kalman filter algorithm in the simulation is
also faster.
4. Comparison and Analysis of Experimental Results
The experimental test system consists of a computer, a dSpace semiphysical simulation system,
and a stable platform. The schematic diagram of the experimental system is shown in Figure 13.
The DA interface outputs control commands, which drive the platform motor through the drive.
The AD interface receives the accelerometer signal and the gyroscope signal, which is used for filtering
and constructing feedback for stable control. The stable platform device is shown in Figure 14. It is
a two-axis and two-frame structure which is equipped with sensors such as a MEMS gyroscope and
linear accelerometers.
The
gyroscope is used to measure platform angular
information.
Sensors 2019,
19, xMEMS
FOR PEER REVIEW
12 ofvelocity
21
Linear accelerometers
are Linear
usedaccelerometers
to measure
acceleration
information.
Linear accelerometers
information.
are platform
used to measure
platform acceleration
information. Linear
accelerometers use
placement
to eliminate
the effects of gravitational
acceleration.
use symmetrical placement
tosymmetrical
eliminate
the effects
of gravitational
acceleration.
Figure 13.Figure
Schematic
diagram
experimental
system.
13. Schematic
diagram ofof
thethe
experimental
system.
Figure 14. Diagram of the stable platform.
After that the equipment is installed and debugged, in order to achieve stable control of the
stable platform system, the gyroscope signal must be introduced into the feedback channel to form
a feedback closed-loop system. Then set different command signals according to different
experiments and send them to the driver and motor through dSpace. The gyroscope and line
Figure 14. Diagram of the stable platform.
accelerometer are used to acquire the motion signals of the stable platform to observe the
corresponding experimental results.
After that the equipment is installed and debugged, in order to achieve stable control of the stable
4.1. Filtering Experiment
platform system, the gyroscope signal must be introduced into the feedback channel to form a feedback
4.1.1.Then
Static Filtering
Experiment command signals according to different experiments and send
closed-loop system.
set different
Set the angular velocity command signal to zero. The gyroscope signal of the pitch axis of the
them to the driver and motor through dSpace. The gyroscope and line accelerometer are used to
platform, the acceleration signal, and the filtered signal of the Kalman filter algorithm and the
acquire the motion
signals
stable
observe
theincorresponding
experimental results.
forward
linear of
filterthe
algorithm
are platform
collected. The to
signals
are as shown
Figure 15-19.
4.1. Filtering Experiment
4.1.1. Static Filtering Experiment
Set the angular velocity command signal to zero. The gyroscope signal of the pitch axis of the
platform, the acceleration signal, and the filtered signal of the Kalman filter algorithm and the forward
linear filter algorithm are collected. The signals are as shown in Figures 15–19.
Sensors 2019, 19, x FOR PEER REVIEW
13 of 21
Sensors 2019, 19, x FOR PEER REVIEW
Sensors 2019, 19, 3552
Sensors 2019, 19, x FOR PEER REVIEW
13 of 21
13 of 20
13 of 21
Figure 15. The angular velocity curve after filtering by the Kalman algorithm.
Figure 15. The angular velocity curve after filtering by the Kalman algorithm.
Figure 15. The angular velocity curve after filtering by the Kalman algorithm.
Figure 15. The angular velocity curve after filtering by the Kalman algorithm.
Figure 16. The estimated curve of the drift of the gyroscope.
Figure 16. The estimated curve of the drift of the gyroscope.
Figure 16. The estimated curve of the drift of the gyroscope.
Figure 16. The estimated curve of the drift of the gyroscope.
Figure 17. The angular acceleration curve after filtering by the Kalman algorithm.
Figure 17. The angular acceleration curve after filtering by the Kalman algorithm.
Figure 17. The angular acceleration curve after filtering by the Kalman algorithm.
Figure 17. The angular acceleration curve after filtering by the Kalman algorithm.
Sensors 2019, 19, x FOR PEER REVIEW
14 of 21
Sensors
FOR PEER REVIEW
Sensors2019,
2019,19,
19,x3552
1414ofof21
20
Figure 18. The estimation curve of the drift of the accelerometer.
Figure 18. The estimation curve of the drift of the accelerometer.
Figure 18. The estimation curve of the drift of the accelerometer.
Figure 19. The angular velocity curve filtered by a forward linear algorithm.
Figure 19. The angular velocity curve filtered by a forward linear algorithm.
Figure
19. Figures
The angular
velocity
curvethe
filtered
by a forward
linear algorithm.
It can be seen
from
15–19
that both
Kalman
filter algorithm
and the forward linear
It can be seen from Figure 15-19that both the Kalman filter algorithm and the forward linear
filter algorithm can reduce the gyroscope noise level. The Kalman filter algorithm reduces the variance
filterItalgorithm
canfrom
reduce the215-19that
gyroscope noise
level. Thefilter
Kalman filter and
algorithm
reduces
the
befrom
seen
the
forward
linear
(deg/s
)2 .Kalman
of the can
noise
0.323 (Figure
deg/s) to 0.061 both
The forwardalgorithm
linear filteringthe
algorithm
reduces
variance
of the noise
from 0.323
(deg/s) tonoise
0.061level.
(deg/s)
forward
filtering
algorithm
filter
algorithm
can noise
reduce
the0.323
gyroscope
The. The
Kalman
filterlinear
algorithm
reduces
the
(deg/s)2 to 0.127
(deg/s
)2 . Moreover,
the variance
of the
from
the improved
Kalman
reduces the
variance
of the0.323
noise(deg/s)
from 0.323
(deg/s)
to 0.127
(deg/s)
. linear
Moreover,
the algorithm
improved
variance
of
the
noise
from
to
0.061
(deg/s)
.
The
forward
filtering
filter algorithm can estimate the constant drift of the gyroscope more accurately. From the collected
Kalman the
filtervariance
algorithm
can noise
estimate
the0.323
constant
drift to
of the gyroscope
accurately.
From the
reduces
of the
from
(deg/s)
(deg/s)
.more
Moreover,
the improved
gyroscope data,
the constant
drift of
the gyroscope
is about0.127
0.45 deg/s,
and
the drift estimated
by the
collectedfilter
gyroscope
data,
the
constant
drift
of the
gyroscope
is aboutmore
0.45 accurately.
deg/s, and From
the drift
Kalman
algorithm
can
estimate
the
constant
drift
of
the
gyroscope
the
filtering algorithm fluctuates around 0.45 deg/s with an average error of 0.02 deg/s.
estimated
by
the
filtering
algorithm
fluctuates
around
0.45
deg/s
with
an
average
error
of
0.02
collected
gyroscope
data, the
constant
drift
thefilter
gyroscope
is about
0.45 deg/s,
the
drift
In addition,
the Kalman
filter
algorithm
canofalso
the angular
acceleration
signal, and
which
reduces
with2 an average error of 0.02
deg/s.
estimated
by the filtering algorithm fluctuates around2 0.45
deg/s
2
the variance of the acceleration noise from 176 deg/s
to 36 deg/s2 . At the same time, it can
deg/s.In addition, the Kalman filter algorithm can also filter the angular acceleration signal, which
estimatethe
thevariance
constantof
value
drift of the accelerometer.
average
error
between) the
value
reduces
the acceleration
noise from
176The
(deg/s
to
36 (deg/s
. Atestimated
the
samewhich
time,
In addition,
the Kalman
filter
algorithm
can also
filter
the) angular
acceleration
signal,
2
and
the
actual
value
is
0.1
deg/s
.
it can estimate
the constant
value drift ofnoise
the accelerometer.
The)average
error between
thesame
estimated
reduces
the variance
of the acceleration
from 176 (deg/s
to 36 (deg/s
) . At the
time,
Therefore,
from value
the perspective
of time domain signals, the filtering effect of the Kalman algorithm
and the actual
is
0.1 deg/s
itvalue
can estimate
the constant
value
drift of. the accelerometer. The average error between the estimated
is better than that
of the
linear algorithm.
from
theforward
perspective
valueTherefore,
and the actual
value
is 0.1 deg/s of
. time domain signals, the filtering effect of the Kalman
The power
spectrum
isofestimated
forlinear
the angular
velocity signals before and after filtering.
algorithm
is better
than
the forward
algorithm.
Therefore,
from
thethat
perspective
of time
domain
signals, the filtering effect of the Kalman
The result
is
shown
in
Figure
20.
The power
spectrum
is of
estimated
for the
angular
velocity signals before and after filtering. The
algorithm
is better
than that
the forward
linear
algorithm.
result
is shown
Figure 20.
The
power in
spectrum
is estimated for the angular velocity signals before and after filtering. The
result is shown in Figure 20.
Sensors 2019, 19, x FOR PEER REVIEW
Sensors 2019, 19, 3552
Sensors 2019, 19, x FOR PEER REVIEW
15 of 21
15 of 20
15 of 21
Figure 20. Power spectrum curve before and after filtering.
Figure 20. Power spectrum curve before and after filtering.
Figure 20. Power spectrum curve before and after filtering.
ItItcan
from Figure
Figure20
20that
thatininthe
thefrequency
frequencydomain,
domain,
both
filtering
algorithms
reduce
can be
be seen
seen from
both
filtering
algorithms
cancan
reduce
the
It
can
be
seen
from
Figure
20
that
in
the
frequency
domain,
both
filtering
algorithms
can
reduce
the
power
of
signal
noise,
and
as
the
frequency
increases,
the
reduction
amplitude
is
also
larger.
The
power of signal noise, and as the frequency increases, the reduction amplitude is also larger. The noise
the
power
signal
noise,
and linear
as
the frequency
increases,
amplitude
is also
noise
filtered
by
the
forward
algorithm
is20
updB
to lower
20the
dBreduction
lower
the spectrum
power
spectrum
ofThe
the
filtered
byofthe
forward
linear
algorithm
is up to
than
thethan
power
oflarger.
the original
noise
filtered
by
the
forward
linear
algorithm
is
up
to
20
dB
lower
than
the
power
spectrum
of
the
original
noise.
The
filtered
noise
of
the
Kalman
algorithm
is
up
to
30
dB
lower
than
the
power
noise. The filtered noise of the Kalman algorithm is up to 30 dB lower than the power spectrum of the
original
The
filterednoise.
noise
of thefilter
Kalman
algorithm
is algorithm
upnoise
to 30reduction
dB
than
power
spectrum
of the
original
Therefore,
the
Kalman
filter
haslower
better
noisethe
reduction
originalnoise.
noise.
Therefore,
the
Kalman
algorithm
has
better
capability
from
the
spectrum
of
the
original
noise.
Therefore,
the
Kalman
filter
algorithm
has
better
noise
reduction
capability
from
the
frequency
domain.
frequency domain.
capability from the frequency domain.
4.1.2.
4.1.2.Dynamic
DynamicFiltering
FilteringExperiment
Experiment
4.1.2. Dynamic Filtering Experiment
Set
Set the
the angular
angular velocity
velocity command
commandto
to aa sinusoidal
sinusoidalsignal
signalwith
withan
an amplitude
amplitudeof
of22 deg/s
deg/s and
and aa
Set
the
angular
velocity
command
to
a
sinusoidal
signal
with
an
amplitude
of
2
deg/s
and
a
frequency
of
1
Hz.
The
gyroscope
signal
of
the
platform’s
pitch
axis
and
the
filtered
signal
are
shown
frequency of 1 Hz. The gyroscope signal of the platform’s pitch axis and the filtered signal are shown
frequency
of
in
inFigure
Figure21.
21.1 Hz. The gyroscope signal of the platform’s pitch axis and the filtered signal are shown
in Figure 21.
Figure 21. Filter curves for different filtering algorithms under dynamic commands.
Figure 21. Filter curves for different filtering algorithms under dynamic commands.
21. Filter
different
filtering
algorithms
under dynamic
commands.
As canFigure
be seen
from curves
Figurefor
21,
both the
Kalman
filter algorithm
and the
forward linear filter
As can be seen from Figure 21, both the Kalman filter algorithm and the forward linear filter
algorithm can reduce the noise level in the case of dynamic commands. Among them, the Kalman
filter algorithm
2
Among
2 them, linear
As cancan
be reduce
seen from
21, both
thecase
Kalman
and
the forward
filter
algorithm
the Figure
noise level
in the
of dynamic
commands.
the Kalman
2 to 0.11 deg/s2 . The forward linear
filter
algorithm
reduces
the
variance
of
noise
from
0.56
deg/s
algorithm
can
reduce
the
noise
level
in
the
case
of
dynamic
commands.
Among
them,
the
Kalman
filter algorithm reduces the variance of noise from 0.56 (deg/s
) . The forward
) to
2 0.11 (deg/s
2
2 0.11
filter
reduces
the
ofofnoise
from
0.56
(deg/s
) 0.56
to
(deg/s
. 2The
forward
linear
filter algorithm
the variance
of
the
noise
from
(deg/s
) deg/s
to) 0.31
) .
filteralgorithm
algorithm
reducesreduces
thevariance
variance
the noise
from
0.56
deg/s
to 0.31
. (deg/s
Therefore,
linear
filterthe
algorithm
the
variance
of the
(deg/s
togyroscope.
0.31 (deg/s ) .
Therefore,
improved
Kalman
filter
algorithm
can noise
better
reduce
noise
of) the
the improved
Kalman reduces
filter
algorithm
can
better
reduce
thefrom
noise0.56
ofthe
the
gyroscope.
Therefore, the improved Kalman filter algorithm can better reduce the noise of the gyroscope.
Sensors 2019, 19, x FOR PEER REVIEW
16 of 21
Sensors 2019, 19, 3552
inREVIEW
the above
SensorsThe
2019,movements
19, x FOR PEER
16 of 20
experiments are more conventional movements. In order to
16 fully
of 21
test the performance of the filter, the platform is artificially rotated randomly without the command
The
movements
in
the
above
experiments
arefiltered
more conventional
conventional
movements.
order
signal
control;
the signal
of the
gyroscope
and the
signal are shown
in FigureIn
The
movements
in the
above
experiments
are
more
movements.
In22.
order to
to fully
fully
test
the platform
platform is
is artificially
artificially rotated
rotated randomly
randomly without
without the
the command
command
test the
the performance
performance of
of the
the filter,
filter, the
signal
control; the
the signal
signal of
of the
the gyroscope
gyroscope and
and the
the filtered
filtered signal
signal are
are shown
shown in
inFigure
Figure22.
22.
signal control;
Figure 22. Filtering effect of different filtering algorithms under random motion.
Figure 22. Filtering effect of different filtering algorithms under random motion.
Filtering
effect
different
algorithms
random
motion.
It can beFigure
seen 22.
from
Figure
22 of
that
in thefiltering
case of
randomunder
motion
of the
platform, the two
It can
be seen from
22 thatfiltering
in the case
of random
motion
of the
platform,
twocurves
filtering
filtering
algorithms
stillFigure
have better
effects.
It is also
apparent
from
the twothe
filter
in
It
can
be
seen
from
Figure
22
that
in
the
case
of
random
motion
of
the
platform,
the
two
algorithms
stillthe
have
better filter
filtering
effects. is
It superior
is also apparent
from the
two filter curves
in the figure
the figure that
Kalman
algorithm
to the forward
linear
algorithm.
filtering
algorithms
have better
filtering
effects.
Italgorithm
is also
apparent
from the
twocase
filterofcurves
in
that the
filter
algorithm
is superior
the forward
linear
filter
To Kalman
explore
the still
response
speed
of thetofiltering
to
thealgorithm.
signal
in the
random
the
figure
Kalman
algorithm
is superior
to thethe
linear
filter
algorithm.
To explore
the
responsefilter
speed
of the filtering
algorithm
toforward
theforward
signal
inlinear
the
case
of randomand
motion
motion
ofthat
thethe
platform,
the
Kalman
filter
algorithm,
algorithm,
the
explore
the
response
speed
of the filtering
algorithm
to
the signal
in the algorithm
case of random
of
theToplatform,
Kalman
filter
algorithm,
the
linear
algorithm,
and
commonly
useda
commonly
usedthe
low-pass
filtering
algorithm
areforward
compared.
The
low-pass
filtering
uses
motion
offiltering
thelow-pass
platform,
theare
Kalman
filterThe
algorithm,
the
forward
linear results
algorithm,
and the
low-pass
algorithm
compared.
low-pass
filtering
algorithm
uses
a are
second-order
second-order
filter
with
a bandwidth
of 20 Hz.
The
comparison
shown
in
commonly
used
low-pass
filtering
algorithm
are
compared.
The
low-pass
filtering
algorithm
uses
a
low-pass
filter
with
a
bandwidth
of
20
Hz.
The
comparison
results
are
shown
in
Figure
23.
Figure 23.
second-order low-pass filter with a bandwidth of 20 Hz. The comparison results are shown in
Figure 23.
Figure
Figure 23.
23. Comparison
Comparison of
of response
response speeds
speeds of
of different
different filtering
filtering algorithms.
algorithms.
As
from
effect
of of
the
low-pass
filter
with
a bandwidth
of 20ofHz
Figure
23.Figure
Comparison
offiltering
response
speeds
different
filtering
algorithms.
As can
canbe
beseen
seen
from
Figure23,
23,the
the
filtering
effect
of
the
low-pass
filter
with
a bandwidth
20
is
similar
to
that
of
the
Kalman
filter.
However,
the
response
speed
of
the
Kalman
filter
algorithm
and
Hz is similar to that of the Kalman filter. However, the response speed of the Kalman filter
the
forward
linear
0.01s
faster effect
thanisthe
low-pass
with
a bandwidth
20Hz.
As canand
be
seen
fromalgorithm
Figure
23,isfilter
the
filtering
of
the
low-pass
filter
a bandwidth
of 20a
algorithm
thefilter
forward
linear
algorithm
0.01s
faster filter
than
thewith
low-pass
filterofwith
Therefore,
the
Kalman
filter
algorithm
and
the
forward
linear
filter
algorithm
have
better
response
Hz
is
similar
to
that
of
the
Kalman
filter.
However,
the
response
speed
of
the
Kalman
filter
bandwidth of 20Hz. Therefore, the Kalman filter algorithm and the forward linear filter algorithm
speeds
to
signals
than
the
low-pass
filters
under
the
same
filtering
effect.
algorithm
and
the
forward
linear
filter
algorithm
is
0.01s
faster
than
the
low-pass
filter
with
a
have better response speeds to signals than the low-pass filters under the same filtering effect.
bandwidth of 20Hz. Therefore, the Kalman filter algorithm and the forward linear filter algorithm
have better response speeds to signals than the low-pass filters under the same filtering effect.
Sensors 2019, 19, 3552
Sensors 2019, 19, x FOR PEER REVIEW
Sensors 2019, 19, x FOR PEER REVIEW
17 of 20
17 of 21
17 of 21
4.2.
4.2.Influence
InfluenceofofSignal
SignalFiltering
FilteringononSystem
SystemControl
ControlPerformance
Performance
4.2. Influence of Signal Filtering on System Control Performance
InInorder
ordertototest
testthe
theimprovement
improvementofofthe
thesystem
systemcontrol
controlperformance
performanceby
bythe
thefiltering
filteringalgorithm,
algorithm,
In
order
to
test
the
improvement
of
the
system
control
performance
by
the
filtering
algorithm,
the
thefollowing
followingexperiments
experimentswere
wereperformed
performedon
onthe
thesystem
systemusing
usingthe
theKalman
Kalmanfilter
filteralgorithm
algorithmand
andthe
the
the following
experiments
were performed on the system using the Kalman filter algorithm and the
system
without
the
filter
algorithm.
system without the filter algorithm.
system
without
the with
filter aalgorithm.
For
the the
classical
controller
parameters
for speed
closedclosed
loop are
K are
=
Forthe
thesystem
system
withfilter,
a filter,
classical
controller
parameters
for speed
loop
For
the
system
with
a
filter,
the
classical
controller
parameters
for
speed
closed
loop
are
K
=
0.15
= and
1.5. This
also applied
to the
unfiltered
system.
For the
above For
two the
systems,
KP and
= K0.15
Ki =controller
1.5. Thisiscontroller
is also
applied
to the
unfiltered
system.
above
0.15
and
K
=
1.5.
This
controller
is
also
applied
to
the
unfiltered
system.
For
the
above
two
systems,
the
zero-angle
command
signal
is given; signal
the experimental
results
are shown
in Figure
24-26. in
two
systems, speed
the zero-angle
speed
command
is given; the
experimental
results
are shown
the zero-angle speed command signal is given; the experimental results are shown in Figure 24-26.
Figures 24–26.
Figure 24. The angular velocity curve of the system output with or without a filter.
Figure 24. The angular velocity curve of the system output with or without a filter.
Figure 24. The angular velocity curve of the system output with or without a filter.
Figure 25. The angular acceleration curve of the system output with or without a filter.
Figure 25. The angular acceleration curve of the system output with or without a filter.
Figure 25. The angular acceleration curve of the system output with or without a filter.
Sensors 2019, 19, x FOR PEER REVIEW
Sensors 2019, 19, 3552
Sensors 2019, 19, x FOR PEER REVIEW
18 of 21
18 of 20
18 of 21
Figure 26. Power spectrum of the angular acceleration of the system output with or without a filter.
Figure 26. Power spectrum of the angular acceleration of the system output with or without a filter.
26. in
Power
spectrum
of the
acceleration
the system
with or
without
a filter.
AsFigure
shown
Figure
24-26,
in angular
the case
of usingofthe
same output
controller,
there
is no
significant
difference
in
the
angular
velocity
of
the
system
with
or
without
the
filter.
This
is
because
the
AsAs
shown
in Figures
24–26,
in the
casecase
of using
the same
controller,
there isthere
no significant
difference
shown
in Figure
24-26,
in the
of using
the same
controller,
is no significant
gyroscope
is
less
sensitive
to
the
motion
of
the
platform
than
the
linear
accelerometer.
Therefore,
it
in difference
the angular
of the
systemofwith
without
theorfilter.
Thisthe
is because
the is
gyroscope
invelocity
the angular
velocity
the or
system
with
without
filter. This
because is
theless
can be seen
from
the line
accelerometer
signal
that the
acceleration output noise
filtered
sensitive
to the
motion
of the
than
theplatform
linear
accelerometer.
it canof
bethe
seen
from
gyroscope
is less
sensitive
to platform
the motion
of the
than the linearTherefore,
accelerometer.
Therefore,
it
system
is
significantly
smaller
than
the
acceleration
output
noise
of
the
unfiltered
system
which
can
thecan
linebeaccelerometer
signal
that
the
acceleration
output
noise
of
the
filtered
system
is
significantly
seen from the line accelerometer signal that the acceleration output noise of the filtered
also
be
seen
from
the
power
spectrum
of
the
angular
acceleration
signal.
The
power
of
the
smaller
the acceleration
output
noise
of the unfiltered
system
which
can also
be seen
from
systemthan
is significantly
smaller
than the
acceleration
output noise
of the
unfiltered
system
which
canthe
acceleration
noise
the
filtered
system
is less
thanangular
the power
unfiltered
power
spectrum;
theofdifference
power
ofof
the
angular
signal.
The
of the
acceleration
noise
theoffiltered
also spectrum
be seen
from
the
poweracceleration
spectrum
of
the
acceleration
signal.
The
power
the is
at
most
2.5
dB
at
250
Hz.
During
the
experiment,
under
the
same
controller
and
command
signal,
system
is less than
unfiltered
at most
2.5 dB at
Hz. During
acceleration
noisethe
of the
filtered power
system spectrum;
is less thanthe
thedifference
unfiltered is
power
spectrum;
the250
difference
isthe
unfiltered
system
have
slight
jitter,
which
is under
the cause
of thethe
above
phenomenon.
atexperiment,
most 2.5
dB under
at will
250 Hz.
During
the
experiment,
the
same
controller
and system
command
the
the
same
controller
and command
signal,
unfiltered
willsignal,
have the
slight
The
above
phenomenon
indicates
that
the
controller
parameters
applied
to
the
unfiltered
unfiltered
will have
slight
jitter,
which is the cause of the above phenomenon.
jitter,
which system
is the cause
of the
above
phenomenon.
system
are
notphenomenon
suitable.
Therefore,
the
parameters
need
to be
different
The
above
phenomenon
indicates
that
the controller
parameters
applied
to the for
unfiltered
The
above
indicates
thatcontroller
the controller
parameters
applied
tore-adjusted
the unfiltered
system
are
systems.
The
more
reasonable
parameters
are:
the
controller
parameters
of
the
filtered
system
are
system
are
not
suitable.
Therefore,
the
controller
parameters
need
to
be
re-adjusted
for
different
not suitable. Therefore, the controller parameters need to be re-adjusted for different systems. The more
K systems.
= 0.15 and
= 1.5
, and
the
controller
of
the
unfiltered
K 1.5,
=
TheKmore
reasonable
parameters
are: parameters
the controller
parameters
of
reasonable
parameters
are:
the controller
parameters
of the filtered
system
arethe
KPfiltered
=system
0.15 system
andare
Ki are
=
K
=
0.15
and
K
=
1.5
,
and
the
controller
parameters
of
the
unfiltered
system
are
K
=
0.05
and
K
=
1.
For
the
above
two
systems,
a
square
wave
angular
velocity
command
signal
with
and the controller parameters of the unfiltered system are KP = 0.05 and Ki = 1. For the above twoa
0.05 and
K
1. wave
For and
the
above
two
systems,
square
wave
angular
velocity
command
signal
with
a
amplitude
of =
5 deg/s
a frequency
of command
1 Hza is
given.
The
experimental
results
are shown
in
Figure
systems,
a square
angular
velocity
signal
with
a amplitude
of
5 deg/s
and
a frequency
amplitude of 5 deg/s and a frequency of 1 Hz is given. The experimental results are shown in Figure
27.
of 1 Hz is given. The experimental results are shown in Figure 27.
27.
Figure
27.
The
step
response
of aaa system
systemwith
withoror
orwithout
withouta aafilter.
filter.
Figure
system
with
without
filter.
Figure27.
27.The
Thestep
step response
response of
AsAs
can
be seen
from
Figure
27,
for
the
step signal, both
both systemscan
can bettertrack
track
the
command
canbebeseen
seenfrom
fromFigure
Figure 27,
27, for
for the
the step signal,
thethe
command
As can
signal, bothsystems
systems canbetter
better track
command
signal;
the
systems
with
filters
have
a
faster
step
response
speed
than
systems
without
filters.
signal;
the
systems
with
filters
have
a
faster
step
response
speed
than
systems
without
filters.
signal; the systems with filters have a faster step response speed than systems without filters.
Sensors 2019, 19, x FOR PEER REVIEW
Sensors 2019, 19, 3552
19 of 21
19 of 20
Since the stable platform is often affected by disturbances such as bumps, sway, and jitter of the
carrier
work,platform
it is necessary
improvebythe
ability of thesuch
stable
platformsway,
to suppress
the of
Sinceduring
the stable
is oftentoaffected
disturbances
as bumps,
and jitter
disturbance. To compare the ability of the stable platform to suppress disturbances before and after
the carrier during work, it is necessary to improve the ability of the stable platform to suppress the
filtering, set the zero angular speed command signal, and apply an angular velocity disturbance of
disturbance. To compare the ability of the stable platform to suppress disturbances before and after
ω = 2sin(2πt) to the stable platform. The angular error of the platform obtained is shown in Figure
filtering, set the zero angular speed command signal, and apply an angular velocity disturbance of
28.
ω = 2 sin(2πt) to the stable platform. The angular error of the platform obtained is shown in Figure 28.
Figure 28. The angular error of the system with or without a filter under disturbance.
Figure 28. The angular error of the system with or without a filter under disturbance.
As can be seen from Figure 28, in the case of external disturbance, the variance of the angular error
As can be seen from Figure 28, in the case of external disturbance, the variance of the angular
of the system without filter is 0.004 deg2 and the variance of the angular error of the system with the
error of the system without filter is 0.004 deg and the variance of the angular error of the system
filter is 0.002 deg2 . Therefore, the system with a filter has a stronger ability to suppress disturbances.
with the filter is 0.002 deg . Therefore, the system with a filter has a stronger ability to suppress
In summary, the controller parameters are optimized due to the filtering and compensation of the
disturbances.
sensor In
signal
by thethe
Kalman
filterparameters
algorithm. are
Thereby
the control
of the
system andofthe
summary,
controller
optimized
due to performance
the filtering and
compensation
system’s
ability
to suppress
disturbances
are improved.
the sensor
signal
by the Kalman
filter algorithm.
Thereby the control performance of the system and
the system’s ability to suppress disturbances are improved.
5. Conclusions
5. Conclusions
In this paper, based on the large noise and drift of MEMS gyroscopes in engineering applications,
a Kalman
algorithm
based
is proposed.
It isgyroscopes
compared in
with
the forward
In filter
this paper,
based
on on
theinformation
large noisefusion
and drift
of MEMS
engineering
linear
filter
algorithm
in
simulation
and
experiment.
By
comparison,
it
shows
the
following.
applications, a Kalman filter algorithm based on information fusion is proposed. It is compared with
the forward linear filter algorithm in simulation and experiment. By comparison, it shows the
1.
Both filtering algorithms can reduce the noise level of the MEMS gyroscope. However, the filtering
following.
effect of the Kalman filter algorithm based on information fusion is better than that of the forward
1. linear
Bothfilter
filtering
algorithms
cannoise
reduce
the noise
level ofcan
thereach
MEMS
gyroscope.
However,
the
algorithm,
and its
reduction
capability
up to
30dB. And
it can estimate
filtering
effect
of
the
Kalman
filter
algorithm
based
on
information
fusion
is
better
than
that
the constant drift of the MEMS gyroscope more accurately. The average error betweenofthe
the forward
filter
algorithm,
and its noise reduction capability can reach up to 30dB. And
estimated
andlinear
actual
values
is 0.02 deg/s.
it can estimate the constant drift of the MEMS gyroscope more accurately. The average error
2.
The Kalman filter algorithm based on information fusion can simultaneously reduce the noise
between the estimated and actual values is 0.02 deg/s.
level of the accelerometer signal. It can also estimate the constant value drift of the accelerometer,
2. The Kalman filter algorithm based on information fusion can simultaneously reduce 2the noise
and the average error between the estimated value and the actual value is 0.1 deg/s .
level of the accelerometer signal. It can also estimate the constant value drift of the
accelerometer,
average signal
error between
the estimated
value
and thebased
actualonvalue
is 0.1
The
processing ofand
thethe
gyroscope
by the Kalman
filtering
algorithm
information
deg/s
.
fusion not only improves the detection accuracy of the gyroscope, but also optimizes the parameters of
the system
controller. of
It also
compensates
forby
thethe
drift
of thefiltering
gyroscope,
whichbased
improves
the system’s
The processing
the gyroscope
signal
Kalman
algorithm
on information
slow
rotation
control.
stability
of the photoelectric
stabilitybut
platform
the ability
fusion
not under
only stable
improves
the The
detection
accuracy
of the gyroscope,
also and
optimizes
the to
parameters
of the system
controller.
also compensates
for the drift
of the
gyroscope,
suppress
disturbances
are improved
as aItwhole,
which has important
practical
significance
in which
practical
improves the
system’s slow rotation under stable control. The stability of the photoelectric stability
engineering
applications.
Author Contributions: H.G. designed the filtering algorithm and carried out experimental research on the effect
of the algorithm. H.H. guided the research and proposed the ideas and revisions of the paper.
Sensors 2019, 19, 3552
20 of 20
Funding: This research received no external funding.
Conflicts of Interest: The authors declare no conflicts of interest.
References
1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
Wang, D.S.; Lu, Y.J.; Zhang, L.; Jiang, G.P. Intelligent Positioning for a Commercial Mobile Platform in
Seamless Indoor/Outdoor Scenes based on Multi-sensor Fusion. Sensors 2019, 19, 1696. [CrossRef] [PubMed]
Liu, F.C.; Su, Z.; Zhao, H.; Li, Q.; Li, C. Attitude Measurement for High-Spinning Projectile with a Hollow
MEMS IMU Consisting of Multiple Accelerometers and Gyros. Sensors 2019, 19, 1799. [CrossRef] [PubMed]
Narasimhappa, M.; Nayak, J.; Terra, M.H.; Sabat, S.L. ARMA model based adaptive unscented fading
Kalman filter for reducing drift of fiber optic gyroscope. Sens. Actuators A Phys. 2016, 251, 42–51. [CrossRef]
Donoho, D.L. De-noising by soft-thresholding. IEEE Trans. Inf. Theory 1995, 41, 613–627. [CrossRef]
Qian, H.; Ma, J. Research on fiber optic gyro signal de-noising based on wavelet packet soft-threshold. J. Syst.
Eng. Electron. 2009, 20, 607–612.
Zhang, Q.; Wang, L.; Gao, P.; Liu, Z. An innovative wavelet threshold denoising method for environmental
drift of fiber optic gyro. Math. Probl. Eng. 2016, 2016, 1–8. [CrossRef]
Yuan, J.; Yuan, Y.; Liu, F.; Pang, Y.; Lin, J. An improved noise reduction algorithm based on wavelet
transformation for MEMS gyroscope. Front. Optoelectron. 2015, 8, 413–418. [CrossRef]
Wang, S.J. Research on Servo Control System of Gyro Stabilized Platform. Master’s Thesis, Harbin Institute
of Technology, Harbin, China, 2008.
Zhang, Z.Y.; Fan, D.P.; Li, K.; Zhang, W.B. Study on the filtering method of micro-electromechanical gyro
zero drift data. J. Chin. Inert. Technol. 2006, 4, 67–69.
Park, S.; Gil, M.S.; Im, H.; Moon, Y.S. Measurement Noise Recommendation for Efficient Kalman Filtering
over a Large Amount of Sensor Data. Sensors 2019, 19, 1168. [CrossRef] [PubMed]
Li, L.M.; Zhao, L.Y.; Tang, X.H.; He, W.; Li, F.R. Gyro Error Compensation Algorithm Based on Improved
Kalman Filter. J. Transduct. Technol. 2018, 31, 538–544, 550.
Zhou, X.Y. Research on Target Positioning Error Analysis and Correction of Photoelectric Detection System.
Ph.D. Thesis, National University of Defense Technology, Changsha, China, 2011.
Li, Q.G.; Zhang, H.L.; Hao, J.R. Research on Gyroscope Drift Compensation Algorithm Based on Six
Accelerometers. Sens. Microsyst. 2009, 28, 42–44.
Jiang, X.Y.; Zhang, X.F.; Li, M.W. Random Error Analysis Method of MEMS Gyroscope Based on Allan
Variance. J. Test Meas. Technol. 2017, 3, 190–195.
Xiong, B.F. Modeling and Correction of Low-Cost MEMS Gyroscope Random Drift Error. Master’s Thesis,
Southwest University, Chongqing, China, 2017.
Cao, H.; Lü, H.; Sun, Q. Model Design Based on MEMS Gyroscope Random Error. In Proceedings of the
2015 IEEE International Conference on Information and Automation, Lijiang, China, 8–10 August 2015.
Zhou, X.Y.; Zhang, Z.Y.; Fan, D.P. Improved Angular Velocity Estimation Using MEMS Sensors with
Applications in Miniature Inertially Stabilized Platforms. Chin. J. Aeronaut. 2011, 24, 648–656. [CrossRef]
Fu, M.Y.; Deng, Z.H.; Yan, L.P. Kalman Filtering Theory and Its Application in Navigation System; Science Press:
Beijing, China, 2010; pp. 38–43.
Anderson, B.D.O. Stability properties of Kalman bucy filters. J. Frankl. Inst. 1971, 29, 137–144. [CrossRef]
Zhou, Z.W.; Fang, H.T. Kalman Filtering Stability with Random Coefficient Matrix under Inexact Variance.
Acta Autom. Sin. 2013, 39, 43–52. [CrossRef]
Yi, K.; Li, T.Z.; Wu, W.Q. Application of FLP Filtering Algorithm in Fiber Optic Gyro Signal Preprocessing.
J. Chin. Inert. Technol. 2005, 24, 60–64.
Zhang, W.B. Research on the Working Characteristics of the Seeker Servo Mechanism and Advanced
Measurement and Control Methods. Ph.D. Thesis, National University of Defense Technology, Changsha,
China, 2009.
© 2019 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access
article distributed under the terms and conditions of the Creative Commons Attribution
(CC BY) license (http://creativecommons.org/licenses/by/4.0/).