Академический Документы
Профессиональный Документы
Культура Документы
Lets find which solution verifies both conditions. Taking p1 > 0, p2 > 0,
R Qa 2 RQb
p
=
p2 = RQb
q
p = Q Q 2RQ
4
b
a
b
F P + P F T P C T R1 CP + Q = 0
where
F =
0
0
1
0
C=
,
P =
Q=
p1
p2
Qa
0
p2
p4
0
Qb
p1 p4 p22 = Qa + RQb> 0
p
R Qb > 0
p1 + p4 = Qa 2 RQb
Once we put the matrices above into the Riccati equation, we obtain
2p2
p4
p4
0
1
R
p21
p1 p2
p1 p2
p22
Qa
+
0
0
Qb
=0
q
p
=
R Qa + 2 RQb
p2 = RQb
q
p =
Qb Qa + 2 RQb
4
2p2 p1 /R + Qa = 0
p4 p1 p2 /R = 0
2
p2 /R + Qb = 0
where one of the equations appeared twice on the matrices. Solving the system for pi , we find
p2 = pRQb
p1 = R (Qa 2p2 )
p4 = p1 p2 /R
p1 + p4 = Qa + 2 RQb
R + Qb > 0
2.1
K1 s + K2
= 2
S
s + K1 s + K2
s2
= 2
G
s + K1 s + K2
0 and s ,
= G d + K1 S
d = K2 S
lim
= 1, lim
= 0 Low pass filter
s S
s0 S
If we apply the Laplace transform on the previous system, we get
s = G d + K1 S
lim
= 0, lim
= 1 High pass filter
s G
s0 G
sd = K2 S
2.3
where all the variables are now functions of the parameter s, i.e. S S (s). We have converted the ODE into
an algebraic system, and now we just have to rearrange
the terms to solve
the second equation, we ob it. From
Noise response
2.3.1
The denominator of the transfer functions is a second order polynomial. A second order system can be characterized by the damping factor, , and the natural frequency,
n ,
s2 + 2n s + n2
n = K 2
To prove that > 0.707 = 1/ 2, we first raise to the
K2 S
power of 2 the expression for , 2 = K12 /4K2 . Now,
K2 (G sS )
d=
= 2
using the values of K1 and K2 found previously,
s
s + K1 s + K2
G s + (K1 s + K2 ) S
=
s2 + K1 s + K2
2.2
1
= 2
s + K1 s + K2
K1 s + K2
K2 s
s
K2
S
G
Qa + 2 RQb
Qa
R
K12
4 =
=
=
+2
K2
R
Qb
RQb
2
Transfer functions
tion for ,
equality implies that must be greater than 0.707.
=
s2
K1 s + K2
G + 2
S
s2 + K1 s + K2
s + K1 s + K2
2.3.2
Numeric application
Set 1
Set 2
10
K1
0.045
0.027
a [/s]
104
105
i [/s2 ]
K2
n [rad/s]
3.6104
1.9102
106
1.18
0.713
Bode Diagram
50
3
3.1
Magnitude (dB)
SST
GYR
100
Phase (deg)
Simulink Application
90
90
4
10
10
10
Frequency (rad/s)
10
10
Simulink model
Bode Diagram
Magnitude (dB)
20
0
20
40
60
180
SST
GYR
Phase (deg)
= G d.
gyrometer measurement,
3.2.1
50
150
180
3.2
90
90
3
10
10
10
10
Frequency (rad/s)
Simulation
Temporal response
0.2
Estimated Angle
Real Angle
0.18
0.16
Angle []
0.14
0.12
0.1
0.08
0.06
0.04
0.02
0
100
200
300
time [s]
400
500
600
x 10
2.5
Estimated Drift
Real Drift
Drift [/s]
1.5
0.5
0.5
100
200
300
time [s]
400
500
600
0.35
x 10
0.06 /s
0.6 /s
6 /s
0.8
0.3
0.6
0.4
Attitude error []
Angle []
0.25
0.2
0.15
0.2
0
0.2
0.4
0.1
0.6
0.05
Estimated Angle
Real Angle
0
100
200
300
time [s]
400
500
0.8
1
600
1000
2000
Drift [/s]
3000
time [s]
4000
x 10
1000
4
3.5
6000
Estimated Drift
Real Drift
5000
0.06 /s
x 10
2000
3000
time [s]
0.6 /s
4000
5000
6000
Drift [/s]
3.2.3
No SST measurements
havior corresponds to a band-pass filter, centered around
the cross-over frequency.
If we consider an harmonic error on the SST measurements, the results are different. With respect to the
attitude, we can see that the low frequency noise induces more errors than the high frequency noise (figure
6). With respect to the drift estimation, we can observe
that, even if the real drift is constant (we are adding har5
x 10
0.06 /s
0.6 /s
6 /s
Attitude error []
Frequency behavior
2
0
2
4
6
8
To conclude, we have seen the advantages of using sensor fusion to measure the attitude. Combining both sensors, we can increase the accuracy of one single sensor,
as we could verify in section 3.2.2. At the same time, the
filter allows us to estimate the drift of the gyrometer.
Both sensors have many complementary properties, and
the Kalman filter gets the best from both of them.
1000
2000
3000
time [s]
4000
5000
6000
3.4
x 10
Real Drift
0.06 /s
0.6 /s
0.06 /s
3.2
Drift [/s]
2.8
2.6
2.4
2.2
1000
2000
3000
time [s]
4000
5000
6000