MMKC DIENDANTI.

COM

Complementary Filter DCM algorithm

Cm bin IMU v cc l thuyt v b c lng gc quay ba trc
B c lng gc quay ba trc (hay cn gi l IMU Inertial Measurement Unit B o lng qun tnh) l h thng c lng ba gc nghing
Euler trong khng gian ca vt th. Ba gc Euler c nh ngha l gc lnh gia ba trc trn vt th so vi trc chun, chiu dng gc quay
xc nh theo quy tc bn tay phi. H trc chun c quy nh y l NED ( Bc-ng-Hng Xung) gn lin vi mt t, h trc gn lin
vi vt th l h trc Descartes vi ba trc x,y,z c theo h trc ca cc cm bin trn IMU. K hiu ba gc quay ln lt l:
Roll: gc quay theo trc x, k hiu

( )

Pitch: gc quay theo trc y, k hiu

Yaw: gc quay theo trc z, k hiu

)
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

Gi vector J l tch vector c hng ca vector K v vector I:

J K , I
Sau tnh li vector I l tch c hng vector J v K:

= J , K

Sau chun ha cc vector I, 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

Gi vector J l tch vector c hng ca vector K v vector I:

J ' K ', I '

Sau tnh li vector I l tch c hng vector J v K:

I ' = J ', K '

ng thi chun ha cc vector gip vic tnh ton d hn:

I'

( I ')

I'
K'=

( K ')

K'
Gi vector T l tch c hng ca vector K v vector K

T K , K '

ng thi nhn vector vi h s ca Accelerometer ta c vector Delta

Delta T * ScaleA

Tip tc tnh t cc gi tr Gyro ( gia tc gc ) ra gi tr gc:

Gyro

* ScaleG

Delta Delta T
Tng t gi tr cn li: Magneto

T I , I'

ng thi nhn vector vi h s ca Accelerometer ta c vector Delta

T T * ScaleM
Delta

Delta + T

Sau khi tnh c vector Delta t gi tr Accelerometer, Magneto v Gyro

Ta bt u cp nht li cc vector v ta NED:

I ' Delta, I
I I I'

K' Delta, K
K K K'
Gi

ERR l sai s sau khi tnh ton t Magneto v Accellerometer. Ta c sau:

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 tan 2(a32 , a33 )

a sin(a31 )
a tan 2(a21, a11 )
Chuyn i gc Euler sang Degrees

180

180

180

360