BÁO CÁO THỰC TẬP-Hệ thống IMU và và các tính toán liên quan

Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi

  !"






 !"#$%&'()*+,-
.+"/,0
 #$%&'(#)(*+, /0
 12345".26789:45;345<
=*

60<=5-3;4.+'
.*>6?@01A26B4+-'"
2345"*

5>20
 12345,8

-,6C';


5D*,,*



5D<E0F2*


-'8G-)*


="5-3;4.+26<8H>I-,
6C.

<"C/J

<*

5D#8K'
"#I,'*5L

<M

5D*,,

5D
<E26CL".*5L

<"C/0
 N,

<E;G



;



5D*,'2GOP=-5

-,6C


'M2"=+2-0%

5>A6
B"2345<0!?6B"2QRS50AA+
6B"23450
 T

<2"

2*"CD

"
7$#>9;UV56

<2"012>W;D$
#>9"P*X

CY#ZJP*X

CY5U-28H
[K0Q2<;I"P*X

CY5IJP*X


CY
2-28H[K0126<8HB\G
6B"2345<01?=B\'26B"
2 '215;'X

"2QRS50
 *+, /0+#1&#2)+3(04(/0, /05&'./67
 $#>9"P*]CY^_"<
 Q55$6C"2345L8#/49-: ;8<:<'7"
<(5DB`(45D#\a>b62c',C
D45D<>b62d'",CD45Da>b62
e0
Page 37
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
F).0Of1<12g5C45345
Q5D6-(89*"*'5D<6-("*"@
12<2345<55$6C";I").0h
>f
 i2c6<"2j5D<JK"$#>9
J-( K
 i2d6<"2j5DaJK"5DkJ5D
<K
 i2e6<"2j$#>#J K"5D
^J5D<K0
 :5H'2c"e2*;"Pl';5X*5
-mRl'ln0o2d2*;"l';5H*5-m['
lnX

"mRlpP'lpPn

 1<2345M*`(5D8G-)f
^E*



5DB

=45#J^'_'kK5)
.0O['QG5Hf
.
>J K Z


P
.
>J K O Z

 
P
>J K>J K Z
 
 

P
P .
>J K p O Z Z

  
Page 38
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm

Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
F).0PF),5Dk(5Dq
 Q,45_#I,J).0OOK0://
,*X

CY5Da"$#>9 '2,
">b"Jh[R

dK">Jh[rdKs>d2f
.
. .
>J K>J K 
>J Ks_ p O 
Y
Z
 



F).0.F),5D_(5Dq
 1U'>W;D"*t6>b26
2345f
Page 39
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
P
P .
.
P
. .

5>J p O K
5>J K
5>J p O K
Z Z
Z
Y Z



  

 
= *+5>8?@0:5&
 ##>9"jP*]CH<"_ku
 Q5$6C"23458-A&8#/49-: ;8<:2'7"-
(5DB`(45D62v',CD45D
62w",CD45D<62x

F).0yi2QRS5J##>9z6-(*""K
12<2QRS5;I").0OP>f
 i2w"2j$#>9"5D^
 i2x"2j##>9"5D"5D_
 i2v{2j5"$#>90
 Q5I,$$\;Di2QRS5<
`"=( !';DD"<
5"C#=*"8'-6B"(5DB`
"-J55{4K
Page 40
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi

F).0|};Di2QRS55"-
 o"25D<J5D;BL*8K
 g"25DJ5D;B*8K
 F4;J_~K"25DaJ5D>>=
5BIK
 B&CD>EF5.4+G
F).0•F(6i*8-gsh[`
 i*8:-"(6*G*`8€I;5-.
+-P5.5D5UJ]>>K;•,(
‚5-P+0
 T;D>W55$6Ci2QRS5f,2gs
h[`-24\I25D^>b5U=5Dk>-I(
<25D_'-2-5D^)>b
5Dk0T€‚65D_"k*"0
 €2g"o>bC8=5/*MJ
RlpP'lpPK0
 -ƒCD6*"C#CC2345'‚2
O;G">W;D(B`-y+J„45K'
>b6+€C9C/.0O0.0
3.1.2 Phương pháp DCM ( Direct Cosine Matrix):
Page 41
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
 …2C/.0O0O'23452<.

<
"C/*




5D*,,

5D*"<E01


"C/"

*

5D'2"^'_'X

k0Q#
\=

"'2*5L

<"C/67
>0
> > [
J ' K > > [ J.0OK
[ [ O
> [ >
J ' K [ O [ J.0PK
> [ >
O [ [
J ' K [ > > J.0.K
[ > >
R Z
R Y
R X

 
  
 

 
  
 

 
 

 
 
 
 
 

 
 

 
 
 
 
 
 
 
 Q52R(Z, Ψ), R(Y, θ) và R(X,Φ) /6"*5€2<
5Da''<42#\v'x'†4C#CC2QR
S50

 N22*5€<t5-.+3;4
>f
> > [ > [ > O [ [
> > [  [ O [  [ > > 
[ [ O > [ > [ > >
xyz
R
   
   
   

     
     
 
     
     

     
Ff
> 0> > 0> > 0> 0> > 0> > 0> 0>
> 0> > 0> > 0> 0> > 0> > 0> 0> 
> > 0> > 0>

xyz
R
           
           
    
  
 

 
   
 
 

 
J.0yK
 L"C#5)6>W;Db-.+
5C4i:J>b+€C5#yK0
 %5€"?6;U<2;I"
>BM‡445*445>b+€C9*D.0.0
 #$%&'(#)(-58/0&>4&
Page 42
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
 …+€C9C/.0O0O0.'C#CC8+;ˆ42345>b<…
5(6i*8:-$…7,CC-y+0
1"(B`„45
 L"

>6C5*95

M

>C\0 26
7//89"B$‡R4!5‰*o~F*
"X*OŠy.'"6\;DCL-.+0
 %

45#267>f

[ O P .
P
P P P P
[ O P .
m n
O
T
q q q q q
q q q q q

    
 1A2-,6C45=*



*


5D>f
[
O
P
.
>J p PK
>J p PK>J K
>J p PK>J K
>J p PK>J K
x
y
z

q
q
q
q

 
 
 




 Q5\5'

"2J58H5;2
K"
>J K
x

'
>J K
y

'
>J K
z

"‹>4=Œ5D
0N2*5L




"5-.
+3;46<>f
P P
P . O P [ . [ P O .
P P
O P [ . O . P . [ O
P P
O . [ P [ O P . O P
O PJ K PJ 0 0 K PJ 0 0 K
PJ 0 0 K O PJ K PJ 0 0 K J.0|K
PJ 0 0 K PJ 0 0 K O PJ K
xyz
q q q q q q q q q q
R q q q q q q q q q q
q q q q q q q q q q
 
   
 
    
 
 
   
 
X

"f
P P P P
[ O P . O P [ . [ P O .

P P P P
O P [ . [ O P . P . [ O
P P P P
O . [ P [ O P . [ O P .
PJ 0 0 K PJ 0 0 K
PJ 0 0 K PJ 0 0 K J.0•K
PJ 0 0 K PJ 0 0 K
xyz
q q q q q q q q q q q q
R q q q q q q q q q q q q
q q q q q q q q q q q q
 
    
 
     
 
 
    
 
H #-:I&CJ>'>K5+)+#L80M+8N5CO
H P*+, /0+#-:I&65&'E580;&245:
N>W;DC#CC2QRS52C#5).0y
<*5€<W28,5=0

> 0> > 0> > 0> 0> > 0> > 0> 0>
> 0> > 0> > 0> 0> > 0> > 0> 0> 
> > 0> > 0>

xyz
R

           
           
    
  
 
 
   
 
 

 

T=="
Page 43
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
of"2†
gf"2x
_~f"2v
H= PE580;&245:+#-:I&65&''*+, /0
i>W2*`*5€<8,5="
OO OP O.
PO PP P.
.O .P
J.0•K
a a a
M a a a
a a a
 
 


 
 
 
QMC#5).0y"C#5).0•0ZGP(>P*
5€6fa
13
= -sinθ ; tanΦ = a
32
/ a
33 ;
tanΨ = a
21
/ a
11
:
F).0•S"52345W*5€<
H P'*+, /0+#-:D&65&'-58/0&>4&
T(tM23458,5=>(B`„45
6<4\>
Page 44
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
>J K>J K>J K >J K>J K>J K
P P P P P P
[
>J K>J K>J K >J K>J K>J K
P P P P P P
O
>J K>J K>J K >J K>J K>J K

P
P P P P P P
.
>J K>J K>J K >J K>J K>J K
P P P P P P
q
q
q
q
     
     
     
     



 
 

 
 

 

 
 
 






 
 
 
 
 
 
 
 

HH P-58/0&>4&+#-:I&65&', /0
ZG(>PC#5).0y".0|6*5€t
>f
[ O P .
P P
O P
[ P O .
[ . O P
P P
P .
PJ 0 0 K
5
O PJ K
5>JPJ 0 0 KK 
PJ 0 0 K
5
O PJ K
q q q q
q q

q q q q
q q q q
q q




 
 
 
 
 
 
 
 
 
 
 

 
 
 
 
 
1A"5"5>‚55-MmR

pP'

pPn0
TŽ,C"*58H"*P28

,-3;4.+0N2\P0O|59"f
P P
[ O P . O P
[ P O .
P P
[ . O P P .
 PJPJ 0 0 K'O PJ KK
5>JPJ 0 0 KK 
 PJPJ 0 0 K'O PJ KK
q q q q q q
q q q q
q q q q q q



 
  
 
 
 
 
 
 
 
 
  
 
 
HQ P-58/0&>4&+#-:I&65&'E580;&245:
Q4C#5).0|".0•

HR P580;&245:+#-:I&65&'-58/0&>4&
1223455=-„45
T(t5I,C#C\C0
Page 45
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
= S
 2)*+

 !'5=,/8,+
*

L

j;D6>W;D57I";?$fT/5TU
0/+G4&>&'0•4;54-"*`€j;U5""5+
';U‚C#CC<5+580
j$265+5*`-$
G$"`8•"`+';22=66
j=€>ƒC=5$'5G+;I"-
(*+0Q="'U=>IC5-B-‘€';4;R
54-59"*`C#CC(;?$0g#
CC";I55((€''5'=
2=665-,,C(0F(
 !"*`\;D)C#CC;4;R54-0
 F( !C5MjX*Oh•[0 $-9
/>IC5"",>7o845i;;5;'*`"C5
(    W    %‘0  N,    I  (*    o845  i;;5;  =  *`  (
5>C4>#…-#;€5"\+ !5,=0 !6\
;D/57IW5D'>2*95`7I"

-;L;D"L>I'€8'"/*'(5+'"
5"-B\+58I"0
Page 46
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
='-:V&.W#4X8CY
Z
&'+[5S
 ` !"t6C(689
*`#G$%&J45*4>54*4>K'8,
5($('>2>W;DC#CC;4;R54-
=65-,,C(01(689%&Z*
2€2";"(0%&GM*8,
J445*445K"*8,50Q(;"6B+89
445*445'?(€268950’LA
CL8(%&" !0%&‚25?"$j>(
'? !)8Z*%&"j€t6C"<
5(01*8,5€2(=*`
(B`*,0SH>W;DC#"=B`"
58/('>2GCL5€26'
26C#=(*V$*01*8,
6;"(5*`(B`*,'"$"
*`(B`ƒ+=*`€`-445*4456ƒ
("`U(0
 Q'8H<€2";"=(
B``ƒ=('""2<6
;"(5(B`0QI(CECCL
2<6€('>2
CL/j>b65(5(B`'
=+-("<65'€28/(5

B`0
 €'(GC5'€'="
€2(8H€2";"(
5(B`*,0“*

 !"
-/

*,M8"5

<5'
='"L



*

-2…6-9

<0
==B-8X4+[5S
 F( !Z*445*4452J]
*8,5K2(5-Z*2
~'C"50 5)8;=)C"2*8
-A<'5"2*8'?~"2
*85DY\0
Page 47
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
F).0•f12_~'g"o

 1?445*445;">b6ƒ85D(
B``;"#\(0!2*
>b565((0%`( !
`/58+*]5GC66C* CEC(‚
!45(2D‚+L*5G-;JCEC
(‚!456C*89!45X*OhP.0gEC(‚
!45*8j= !"A5(B`ƒ
+=L*5G;UL**`€;58+*]5G
8t"-?5U=L*5G899)0T"
,"…6\;Dj(;?$`
/8+*]5G5"'*8”K0 L

 !)8Z*


*8,$"j€t6C"<
5(0
F).0Šf!It5*8\=2_~'g"o
Page 48
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
== #A&.4X>S
 F("/6G89*`8`%&"€
  ;4;R54-0  QU  `  "    8  5    *  8,  5  "
445*445*"2CL !"f*84;>>4*"
>5C;~>>4*0
 =i*84;>>4*'8445*4456>ƒC<,C485D
2="/6‚4C#8ƒR*'RL"#
L*-@L*5G0*8C#=445*445-
t-‘(t)445*44565

*`#G+B"5R>8a4;C{5*01#G+2Z*28*8'
"j?2*`5D$-?'"
*8"6-,=/6M""5>*V
*825D0’*85U'8*8,568
545D#I445*445'"2\X8@(6
C544>> J>It55D*8K';2*8
C#=*8-t0:*8,5$>W;D
5*84;>>4*$"5CL#5DJ45554
5K'"(•56"*``#>45*8#\
+->C#=*8-t5(B`
0i*85U2C#=-t55)
`('445*445"565*`*]
CY+5*8"01G*84; !I>I5GC\C'(
,6-,G*86`<"I-)-2-X'
+";•,("*84; !5G0
 !5C;~ !2-,G#-##'C/W
`#>=*84; !'89)5(>5C;~-+2-,
G*80Q5>5C;~ !'445*445"56
5-,G('0Q5>5C;~ !'j\;D
(5<W("5G5B'"j-(
>5C;~$"*8,(W'B8;•0Q5>5C
;~ !-2"C/""`+*]€'5
(5(,ƒ+=(689*
8,5J*">569L"€2
5D#\K01445*4456>W;D;"(
45D5(B``0!5C;~ !5
6-,6C=CEC8,tB`>565"5
(0
 T=tC#=*8)*84; !
]CC>>-=-(t‘*```*"

Page 49
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
*8t-C$'(6"B"*8-0=>5C
;~ !)-<5(650
 Q`€C€;j(>5C;~ !#*84; !
5G+/0i*84; !2`=`C;4"|[R•[/p
L'=>5C;~8)$"P[[[/pL0T"/*
5>W;D5>5C;~ !=#5*84; !0
=== %&\]C4.$^&'9-)&81&#3&/08>5.E/56-0/E/&8-&>87
 #$"*

,8

W6;U"
55L

'='5B5$*

C#




')"*80%&>W;D>I-,6Cj*8,
"*8,50%&$6;U+-5)


C#


L

X

58I"0
 #$""C/


 !6>W;D5/,*8'"'"/*'C
+5D'C#

L

$8

"558I
"01;j

L

C6M*8,5%&CEC*
2<5C#



'>W;DC#CC
68,,;=B;4;R54-0
 %&

8H<5


->W
;D*

X

+*8,0 2C

jt
2o'g"_~>W;D*

X

+*8,50
 Q5

';j

6M*8,>b6
"*"55

;I5L

"$0
F).0hf12_~'g"o5


 6*#8G%&5

2"A

$8V0S9)

;•$D*"j
t6C

"5…65=2'8G-)*

>>"
55)$';U"@'>b6;/;/0+";•
,

56'X

*

>It-8

""
Xj55*"

"5I,,80
Page 50
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
 %&$‚"*

"C/

01


->b6>W;D"*<5>

*"%&*ƒ
C55)

,8')


ig!'*8,I5B5$'*8,L

8"J+8U>I5L


K'

-C

‚<5

'"*

*8,
8"

WAC<M5$0
 1G5A%&$8Z**

-=.*8,
".*8,501*8,6X


>5D
A5I=01A')"5B
5$01*8,56X

5

5I#I'5
22*,,j

C6C…6B0
@_`
Z
a
 %

#$2689+*
8,-H*4*

<

0Q'
*

  #      $      #  8  >b  8  Z*  *  8,    
J445*445K"*8,5…5)8"95"22*
8,M5$J*4*445K0
 bEF>c&:04
 Q€j5>C4\Z<G(Mj,-‚
^^'"5j€C/L26>W;D5`5…"6
,5"/=M 5. j,8/+Z

6C;D+`*`€
Q5G0T";/;/5…6C5"\;D5`5…5+
7I'"]8("5( !0125
".=`'G"-X\;D-
>f5#'5'"5(0N9/5>IC5
"5#'`;I5Z5+0
i5#6>W;D+5(*84; !0!2">I5$
5B'"/LG">IC55(
\ ;D89 (  #  (  W %3%!  J*5R445*4
>>4*>K'5"5(\;D+5>5C;~ !0
 :04+%
 i5#J$B8H€j5~44K`
;I58"**4*`€`-
tI;D€5(0
Page 51
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
 1G#8*`5#Z*2*`7-*)5?
654#W8H*`>6;L"Z5*`8`-07-*
65+`8H`>6;L;"Z54
;B45D8<40N`)iBA "**4
75'I "**47=5D'w "€
75D0•I58"**42f
0 
i i
r r ir
A I w
 =G;75?26**4
75(B`]L*7"5D#\=
5D*80:"5G5#'5BL*7

<C5U=5BL*(5*8f
[ [
[ [ 
[ [
rx
r ry
rz
I
I I
I
 
 

 
 
 
 Q55)`',5(t'
)*82`5D'*"75•
jC#=5D'"j5t5
-01?2*8268H
,82ƒ+=5D*8',8"6B
8H€jC-{{0
 !6*85G5>8€I;
`50T;D528€I;)2GZ*2
*80 ,5D*`58€I;"Y\)2
6B"5Y\J455K'?,5DH*5
*]CYH*)B"5=J;545K'?*`
525D-4*`C#D")B"5I;
J{5445K0!It5-8+?@*`5Y
\"*`5=']"5I;0%`5#2

/*\;D5`5…"5*`8€I;'\"G‚Z**`
*85D*"'5"$6B"455
5450
 i5#"55+'2`<'"6\
;D+5(;•$W"/*0i5#2
\X,"tC#=`(
0i5#,6>W;D5(*84; !89)-,G
#-5GCU6C0F/,( !+>W;D-,6C8*
8,554455*`8€I;tC#=5
-0
= :049-5&'3(8>+5.':047
 `5;I5(\!'6
"€B$gCi454>!)*5X*OhO.'"(\
>#=€0F(\!689*`
Page 52
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
?0%`Z>65""5+
5U*`‘=+56'"‘2"*`
?-'Z$"*`,8'B";DD
?J545{45*445K0Q,8"6ƒ*`–2
60N–)?>b8;';•,L
65*">>b8;'"`;
LU`"`;2–0
 1G5#",>\C\C"Z*+8`C€
`'?5)2G##"/-28`
C€"`01256C5"\;D
5`5…5(;?$`<'2"5?>45"
5C0
 :04\d&'.56/03!U0>&'.56/0':047

 %`5?>45#82G"*`5…-2;)
BJ$"8'8'>K6-85*`-
015…""$`*`V6C-'"4*
"40
 1CLW-`C5>-8-89
(5$55…-01U*>6"`4
=6+55…-'*`U*>`4
+-*ZZ"U*>?`6+-*Z
Z0Q5L8H-')$;
U*>,…$"*`5…-"'
>2>"UC01?,-*`2
5-$U*>,*`5…--?
j89…$2>I>('">2>8(
C0 ,>b"*;L5,8>0
Page 53
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
F).0O[f1G5?>45
F).0OOfi5?>45I,
 :04+)(9-5&'3e>F/04(8>+':047
 %];U>IC55?>45"5G"'=
j\57IC…`Ij,-,
5C0
 —>65+5>6C6G"`'"
+;"2,|-*'+;">6C;"ƒ
>bt`5C0Q$5C2-
Page 54
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
=@'˜">G8E0 "`;LC)5C

?2j"C/#8>f*`Z>'*`8`CLI
>'*`8`Z>'*`8`‚C>"*`*8,
C(>0
 `5C""#I5
?>45'>>-6CLI"6"*>b5+
4=65`C0S`CLI*8
Z>C6CLIZ+`5+8H
0Q5L8H'5-)-2()9*8,
89)U*>"6C5(0 ,5
*`2)U*>(C-,6C>b*`U*>-,6C'
$`U*>-,6C6*8,"(C
#\0(C",2"=0
F).0OPf1"C/#8i5C
 i5C2]65`>=5?>459V2
2CEC=+`-U4\;D89
+;"`C2,60T"U=-=@
B5C6\;D""5`5…5(
";?$0
Page 55
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
F).0O.fi5CI,
 :040-&'3>F058>&'':047
 T="GC#Y>=5-'556
\;D+57I;L;D=0i55,6;U
24;<45A;'"55‚25?V
56ig!'(<4"0
 1(,#G'*8,"(#(
J%3%!R*5445*4>>4*KC525
",='"""6\;D+><G5j,8

=>6=""01*8,55#
J*5*4855K*8,€22+*X
5G=5+\;D"j*8,#>957I;?
$"+-01*8,%3%!$-2`<
""5GGC0Q-X`*8,26
LGC$D-8`B<W(0
 Q5/,55')"C/*8,#8"
*`#GZ**`-6j8H?<0
 N9j6j89?<B"-6IJC5{
*>>K0g5{*>>2;`9*`5,`'*`,`-
-2I5>'B",`#>9?,`?",`\GC
-C5{*>>;`5*`("I5>0
Q$55`9,`\GC'8HtC
(>I;`C5{*>>'26€(0
T€"=)(C"0•C5{*>>6GM€(
C(55895G=89(`0T=(
%3%!'(6"…6,';2X55"
6L0
= bEF>c&'>58f+
Page 56
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
 %`-,>W;DO€>Itj
`B5->=5B5$0
 i-,;ƒ2G-55'Z*O
€]JC5{*>>K65489O?<0T€]2`;B
4?<01ƒ6]"*5$*G,9
5`0F)>*BGO-,ƒ
F).0Oyf1G-,ƒ
 ,2O)?<>b8,;0•I5`8,;

?<*"26(02"`
-,ƒ0
 F('$$;U-,?-'"*


-,2-X"*(#Y-,;ƒ"/
-2"C/";'$ƒ**

`;L8"
C5{*>>0 `2"-2;@C5{
*>>)>b>5O;?(5`;L'*

I(M4+
6-W`0•2'2>56
`(8H;?(5`;L0
 T= >I C  5     (  %3%!J*5445*4
>>4*>K)-,6><G"="GC"
G6-0
bEF>c&8P80$^&'
Page 57
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
 1*8,M5$"*`,8;U;$`"=
M5$J655C?(*]M5$5GK0
 #5($,"Q4>0 L"#
-=$;U#4>JQK0Q5I,)$
M5$=#"i>>0
 Oi>>sO[[[[[JQK
 QM5$5G"*`5$,0 2t45G+
L-0QM5$5G2$`-P[[[[Q9

<"-Š[[[[Q9/I0
 1*8,M5$65"*P"f
 !5*4*445ft$`M5$0
 T45*4*445fM"C/M5$4
5D',-X=,80
H ghijkS@lm@_
Q5;=L+>W5H€ƒ,82B`("
^
8
z_
8
zk
8
z'5-B`*8,JB`#K"^
8
_
8
k
8
f
%(j(5™`("(5DB`#6<
>J4\.0yK=
T
xyz
R
"*5€
xyz
R
f
š

š 
š
b b
T
b xyz b
b b
X X
Y R Y
Z Z
   
   

   
   
   
š
> > > > >
š > > > > > > > > > > > > J.0•K
> > > > > > > > > > > >
š
b b
b b
b b
X X
Y Y
Z Z
    
           
           


   
 
   
 
  
   
 
   
 
  
 
   
H gn.WTK.>L-8P+bEF>c&80N&'80$^&'35++/ /04E/8/07
Page 58
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi

F).0OyQ22JKM445*445
 QW445*4452<62(
8H<2o"g0

Q5(B`#^
8
_
8
k
8
'2555$6C
*8,]2=C#5BI>f
^

8
s_
8
s["k
8
sOu
5(5DB`(>b65"‡<'‡
'‡
a
'5"…6q2J5*a4K0
Q"C#5).0•6
> > > > > [
> > > > > > > > > > > > [
> > > > > > > > > > > > O
x
y
z
A
A
A
    
           
           
 

   
 
   
  
 

   
 
   
  
   
 
QML2>5"f
i2os†s‡5>J‡

p>xK
i2gsxs‡5>JR‡
<
K
 1A5H‡
<
'‡

'‡
a
…6q2'7"
P P P
O
x y z
A A A  
Page 59
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi
,8\"-@)*8,"-,]
2>>5(80
H=gn.WTK.>L-8P+bEF>c&8P80$^&'

F).0O|fQ2<J_~KM*4*45

5)8"95…<62$,*
8,445*4450SL$>b<2<4C#
J_~K4*8,M5$01<65)8">f

iB%<'%'%a")+45$`M5#S
5D``#"%z<'%z'%za")"565
(B`(0

Q5(B`()5•5"_~>b6>f
_~svs5J%zp%z<KJ.0ŠK

,,842o"g)+(B`
(>f
š
š J ' K J ' K J.0hK
š
x x
T T
y y
z z
M M
M R X R Y M
M M
 
   
   

   

   
   

Q\.0P".0.".0h6-,f

%z
<
s%
<
>x›%

>x
Page 60
Chương 2: GVHD: ThS. Huỳnh Văn Kiểm
Hệ thống IMU và và các tính toán liên quanSVTH: Lê Văn Mùi

%z

s%
<
>†>x›%

>†R%
a
>†>x

%z
a
s%
<

>†>x›%

>†R%
a
>†>x

1\.0Š25$6C>f
 _~svs5J%zp%z<KJ,%<œ["%œ[
 _~svsOŠ[›5J%zp%z<KJ,%z<•[K
 _~svs.•[›5J%zp%z<KJ,%z<œ["
%z•s[K
 _~svsh[

J,%z<s["%z•[K
 _~svsP•[J,%z<s["%zœ[K

1A5H#I5$6C*8,2%z<'
%z'%za6q2'7"
P P P
š š š O
x y z
M M M  
,+"-@7"…2+MZM5#
-0
Hgn.WTK.>L-8n+bEF>c&\;&8f+'*+3:047

T(>(Mi5"-C\C)25;j("
€2<62)ACCL4
$0


1C#5);U€ZJCLK2345M
i5>f
[ > >
O
[ > > > >
>
> > > > >
x
y
z
t
t c
t



 

    
 
    



 
 
 
 
 
 

 
 
 
 
 
 
 
 
 
 
 
 
 
 
T=v'x'†/6"2345$*G*•5=2JROK0
i2345$*(6<>f
Page 61