Professional Documents
Culture Documents
Complementary Filter DCM Algorithm PDF
Complementary Filter DCM Algorithm PDF
COM
( )
)
2
( )
2
Mt th thng IMU thng thng s gm ba loi cm bin Gia tc ( Accelerometer ), vn tc quay (Gyroscope ) v T trng (Magnetometer).
Trn IMU c th thm cm bin nhit b nhit cho cc cm bin hay cm bin p sut o cao
Vic c cc gi tr cm bin i hi phi a qua mt b lc a ra cc gc Euler, mt s thut ton cho b lc: b lc b (
Complementary Filter ) v lc Kalman (Kalman Filter ) hay b lc AHRS.
B lc b c u th hn th tnh ton n gin cng nh thi gian tnh ton t.
Sau y s trnh by thut ton b lc b ( Complementary Filter ) a ra cc gc Euler
Khi to cc ma trn:
Vi 9 gi tr c v:
Magneto [ X Y Z]
Accelerometer [ X Y Z]
Gyroscope [X Y Z]
Gi vector I l vector i din trc X (North )
Vector K l vector i din cho trc Z ( Download )
Ta s c nh sau:
I0
I1
I
2
K0
K1
K
2
Magneto X
= MagnetoY
Magneto
Z
AccelerometerX
= AccelerometerY
Accelerometer
Z
J K , I
Sau tnh li vector I l tch c hng vector J v K:
= J , K
(I )
(J )
J
K
(K )
K
Ta a c ta vt v ta NED.
Cp nht gi tr ta :
Sau khi khi to I, J, K ta cn phi tnh thm gi tr Delta v tnh c ta hin ti
Gi I, J, K l cc vector o c ln tip theo:
Ta s c nh sau:
I '0
I '1
I'
2
K '0
K '1
K'
2
Magneto X
= MagnetoY
Magneto
Z
AccelerometerX
= AccelerometerY
Accelerometer
Z
I'
( I ')
I'
K'=
( K ')
K'
Gi vector T l tch c hng ca vector K v vector K
T K , K '
Delta T * ScaleA
Gyro
* ScaleG
Delta Delta T
Tng t gi tr cn li: Magneto
T I , I'
T T * ScaleM
Delta
Delta + T
I ' Delta, I
I I I'
K' Delta, K
K K K'
Gi
ERR I K *
1
2
I ' I .ERR
K ' K .ERR
Sau chun ha cc Vector:
1
I I (3 I .I )
2
1
K K (3 K .K )
2
J KI
Sau cp nht tip cc gi tr cm bin a v vi thi gian DeltaT
T ma trn xoay mun a v cc gc Euler ta dng cng thc sau:
Vi cc vector I,J,K ta thu v c ma trn DCM:
I
J
K
I11
= J 21
K
31
I12
J 22
K32
I13
J 23
K33
a sin(a31 )
a tan 2(a21, a11 )
Chuyn i gc Euler sang Degrees
180
180
180
360