Proceedings VCM 2012 66 ứng dụng bộ lọc kalman phi tuyến mở rộng xây dựng

488 Trần Đức Thuận, Bùi Hồng Huế, Trương Duy Trung, Trần Xuân Kiên, Nguyễn Sỹ Long
VCM2012
Ứng dụng bộ lọc Kalman phi tuyến mở rộng xây dựng
thuật toán xác định tham số định hướng trên cơ sở kết hợp
con quay tốc độ góc với từ kế và gia tốc kế
Applying none-linear extended Kalman filter to construct
a algorithm for calculate directional parameters based on MEMs
Trần Đức Thuận, Bùi Hồng Huế, Trương Duy Trung, Trần Xuân Kiên, Nguyễn Sỹ Long
Viện Khoa học - công nghệ Quân sự
e-Mail: ;
Tóm tắt
Bài báo trình bày phương pháp ứng dụng bộ lọc Kalman phi tuyến mở rộng để xác định các tham số định
hướng cho các vật thể dịch chuyển trên cơ sở kết hợp con quay tốc độ góc với từ kế và với gia tốc kế theo
phương pháp ghép chặt
Abtract:
This paper presents a methodology utilizing nonlinear extended Kalman filter to determine the orientation
parameters for moving objects based on MEMs according to rigidly mounted method

1. Phần mở đầu
Trong các bài toán dẫn đường hoặc điều khiển các
vật thể chuyển động (bao gồm chuyển động dịch
chuyển của tâm khối và chuyển động quay) luôn
có hai vấn đề, đó là vấn đề định hướng và vấn đề
định vị. Vấn đề định hướng là xác định tương quan
về góc giữa vật thể và hệ tọa độ chuẩn (hệ tọa độ
được lấy làm gốc để đánh giá sự chuyển động), tức
là tương quan giữa hệ tọa độ liên kết và hệ tọa độ
chuẩn. Vấn đề định vị là việc xác định vị trí tâm
khối và vận tốc của nó so với hệ tọa độ chuẩn.
Trong bài báo này chỉ xem xét vấn đề định hướng.
Việc sử dụng thông tin về tốc độ góc của các con

quay vi cơ cung cấp để xác định các tham số định
hướng đã được nhiều sách giáo khoa của nước
ngoài trình bày [1], đã có một số bài báo ở Việt
Nam đề cập. Tuy nhiên, thông tin về tốc độ góc do
con quay vi cơ đưa ra luôn có các thành phần sau:
tốc độ góc quay thực tế, nhiễu đo và độ trôi (thành
phần biến đổi chậm), vì vậy việc xác định các
tham số định hướng theo phương pháp trực tiếp
luôn có sai số và sai só tăng theo thời gian. Đã có
một số công trình ở nước ngoài sử dụng thông tin
của từ kế và gia tốc kế để khắc phục vấn đề sai số
vừa nêu, song chủ yếu dùng bộ lọc Kalman tuyến
tính để ước lượng các sai số. Có thể có những
công trình quân sự đã sử dụng phương án ứng
dụng bộ lọc Kalman phi tuyến mở rộng để kết hợp
con quay vi cơ với từ kế và gia tốc kế, song chúng
không được công bố. Vì vậy việc xây dựng thuật
toán này là cần thiết trong giai đoạn hiện nay đang
có nhiều ứng dụng các phần tử vi cơ cho UAV, tay

máy, cơ cấu dịch chuyển cơ khí, phương tiệm
ngầm, tầu thủy v.v
2. Ma trận cosin chỉ phương và các góc định
hướng
Giả sử có vật thể chuyển động B với hệ tọa độ liên
kết OXYZ (hệ tọa độ gắn liền với vật thể) được
xét trong hệ tọa độ chuẩn A là
0000
ZYXO . Dịch
chuyển tịnh tiến gốc hệ tọa độ chuẩn đến tâm khối

vật thể (các trục
00
XO ,
0000
, ZOYO luôn tự song
song với với chính nó trong quá trình dịch chuyển-
hình 1). Các góc mô tả quan hệ giữa hai hệ tọa độ
này được gọi là góc định hướng, gồm góc hướng

, góc chúc ngóc

và góc Cren

(định nghĩa
các góc này thể hiện ở công trình [2 ]).

H. 1 Hệ toạ độ liên kết và hệ toạ độ chuẩn

Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 489
Mã bài: 115
1 - Mặt phẳng nằm ngang ;2 - Mặt phẳng đối
xứng TBB; 3 - Mặt phẳng đứng chứa trục dọc 4
- Mặt phẳng đứng chứa trục OZ.
Từ các góc định hướng
, ,
  
[2] người ta xác
định ma trận cosin chỉ phương giữa hai hệ tọa độ
như sau:

11 12 13 11 12 13
11 21 22 11 21 22
31 32 33 31 32 33
,
T
a a a c c c
A a a a C C c c c
a a a c c c
   
   
   
  
   
   
   
   
(1)

sin sin
sin sin sin sin sin cos sin
sin sin sin s sin sin sin sin cos
cos cos cos
A cos cos cos cos cos
cos co cos cos
    
           
           
 
 
   

 
 
  
 
(2)

Ma trận cosin chỉ phương cho phép chuyển đổi tọa
độ hoặc véctơ từ hệ tọa độ A sang B và ngược lại,
tức là nếu biết véctơ
( )
T
A A A A
X x y z
 trong hệ
tọa độ A thì véctơ đó trong hệ tọa độ B sẽ có giá
trị
B A
X A X


và ngược lại
1
A B B
X A X CX

  .
Nếu bằng cách nào đó nhận được giá trị của các
phần tử a
ij
của ma trận A từ các biểu thức trong ma

trận A (2) dễ dàng nhận thấy các góc



,, :
32 22 12
13 11
( / ), arcsin( ),
( / )
arctg a a a
arctg a a
 

  
 
(3)
Việc xác định các góc



,, hoặc các phần tử a
ij

hoặc c
ij
chính là lời giải của bài toán định hướng.
Toán học đã chứng minh nếu bằng cách nào đó đo
được tốc độ quay của hệ tọa độ B so với hệ tọa độ
A (hình 2) thì có thể xác định được các tham số
định hướng nếu trên. Thực tế ngày nay kỹ thuật đã

chế tạo được các con quay vi cơ có kích thước nhỏ
cho phép đo các vận tốc góc
zyx

,
, của véctơ
quay của hệ tọa độ B so với hệ tọa độ A nếu hệ tọa
độ A là hệ tọa độ quán tính.











H. 2 Sự quay của hệ tọa độ B so với hệ tọa độ A

Hiện nay có hai phương pháp cơ bản xác định ma
trận A trên cơ sở thông tin về tốc độ góc
zyx

,
, . Phương pháp dùng 4 số Rodring-
Haminton và phương pháp giải trực tiếp 9 phương
trình vi phân mô tả quan hệ các phần tử
ij

c
của ma
trận C với các tốc độ góc
zyx

,
, . Quan hệ
giữa ma trận C và các tốc độ quay
zyx

,
,
được [2] thể hiện như sau:



C C



(4)
trong đó [ ]

là ma trận phản đối xứng:


 
0
0
0

z y
z x
y x
 
  
 

 
 
 
 
 

 

(5)
Từ (4) và (5) có 9 phương trình vi phân sau:
X
0
Z
0
Y
0
Y'
0
Y

X'
0
Z'

0
X

Z

y


x


z


490 Trần Đức Thuận, Bùi Hồng Huế, Trương Duy Trung, Trần Xuân Kiên, Nguyễn Sỹ Long
VCM2012

11 12 13 12 13 11 13 1 1 12
21 22 23 22 23 21 23 21 2 2
31 32 33 32 33 31 33 31 32
, , ,
, , ,
, , ,
z y x z y x
z y x z y x
z y x z y x
c c c c c c c c c
c c c c c c c c c
c c c c c c c c c
     

     
     
     
     
     
  
  
  
(6)


Từ hệ phương trình (6) cho thấy nếu có các điều
kiện ban đầu
0
( )
ij ij
c o c

và có thông tin
zyx

,, dễ dàng xác định các hàm
ij
c
. Song
trong thực tế các tốc độ góc
zyx

,, do con
quay (phần tử đo) cung cấp thường có dạng sau:

0
1
0
2
0
3
x x x
y y y
z z z
b w
b w
b w
 
 
 
  
  
  
(7)
Trong đó
o
x

,
o
y

,
o

z

là tốc độ quay thực của hệ
tọa độ B, b
1
, b
2
, b
3
là các tham số biến đổi chậm
thể hiện độ trôi của con quay. Còn
, ,
x y z
w w w
là
các nhiễu đo thường có dạng ồn trắng. Như vậy
nếu dùng thông tin của các con quay đưa vào
phương trình (6) để giải chúng sẽ nhận được các
hàm số
( )
ij
c t
(i = 1, 2, 3,
j
= 1, 2, 3) không đúng
với thực tế. Để khắc phục hiện tượng này bài báo
đề xuất một phương pháp dùng bộ lọc Kalman trực
tiếp với quan sát là các giá trị do từ kế, gia tốc kế
và các con quay vi cơ đo tốc độ góc cung cấp để
xác định các hàm

( )
ij
c t


của ma trận C.
3. Bộ lọc Kalman phi tuyến mở rộng và
thuật toán xác định ma trận cosin chỉ
phương.
Bộ lọc Kalman là một công cụ toán học giúp
cho việc đánh giá vec tơ trạng thái hệ động học
trên cơ sở thông tin quan sát véc tơ tín hiệu đầu ra.
Giả sử quá trình chuyển động của vật thể hoặc quá
trình công nghệ được mô tả bởi hệ phương trình
động học dưới dạng rời rạc như sau:

1 1
( )
( )
k k k k
k k k
X F X
Z h X v

 
 
 
(8)
1
,

kk
XX là trạng thái của véctơ trạng thái X
(véctơ n chiếu) ở bước thứ k và bước thứ (k-1),
k
Z
là giá trị của véctơ đầu ra (véctơ m chiếu,
thường m

n). Véctơ đầu ra này có thể đo được
bằng các phương tiện đo.
1
k
F

là véctơ hàm số
F

ở bước thứ k-1:

T
n
fffF ), ,(
21
 (9)
1 2
, ,
n
f f f
- các hàm phi tuyến với biến số là
véctơ X. h

k
là véctơ hàm số h ở bước thứ k:

T
m
hhhh ), ,,(
21
 (10)
kk

, - là véctơ nhiễu động lực và nhiễu đo có
dạng ồn trắng với kỳ vọng toán học bằng 0:

~ (0, )
~ (0, )
k k
k k
N Q
v N R


Ma trận hiệp phương sai của véctơ sai số đánh giá
trạng thái véctơ X như sau:

T
k
k
k
kk
XXXXP ]][([





Ở đây

là ký hiệu kỳ vọng toán học.
Thủ tục của thuật toán đánh giá trạng thái X trên
cơ sở véctơ quan sát (đo được)
Z
theo Kalman [3]
như sau:

1 1
ˆ ˆ
( ) ( ( ))
k k k
X F X
 
  


1
ˆ
1 1
( )
/
k
k k
X X

F X

 
 
   

ˆ ˆ
( ( ))
k k k
Z h X
 


ˆ
( )
/
k
k k
X X
H h X
 
   (11)
1 1 1 1
( ) ( )
T
k k k k k
P P Q
   
     
1

( ) ( ( ) )
T T
k k k k k k k
K P H H P H R

   

ˆ ˆ ˆ
( ) ( ) ( )
k k k k k
X X K Z Z
    
( ) ( ) ( )
k k k k
P I K H P
   

Ở đây I là véctơ đơn vị .
Đối với thiết bị dẫn đường quán tính thường gặp,
việc xác định các phần tử
ij
c
của ma trận C được
thực hiện trên cơ sở giải hệ phương trình vi phân
(6) trên cơ sở thông tin đo được từ các con quay về
tốc độ góc
zyx

,, . Tuy nhiên, kết quả có sai
số lớn khi có yếu tố trôi

1 2 3
, ,
b b b
. Trong bài báo
này nhóm tác giả đề xuất phương pháp ước lượng
các phần tử
ij
c
trên cơ sở dùng bộ lọc Kalman với
thông tin đầu ra từ các từ kế và gia tốc kế gắn trên
vật thể B. Giả sử trên vật thể B gắn 3 từ kế và 3
gia tốc kế theo các trục OX, OY, OZ của hệ tọa độ
liên kết (hình 3).

Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 491
Mã bài: 115







\
H. 3 Sơ đồ bố trí con quay vi cơ đo tốc độ góc,
gia tốc kế và từ kế
T
x
, T
y

, T
z
- các từ kế; G
x
, G
y
, G
z
- các gia tốc kế;
, ,
x y z
  
- các con quay vi cơ
Giả thiết trong vùng hoạt động của vật thể
véctơ cường độ từ trường trái đất và véctơ gia tốc
trọng trường có phương và giá trị không đổi. Khi
hệ tọa độ liên kết trùng với hệ tọa độ chuẩn đo
được các giá trị từ trường
0 0 0
x y z
B B B
và gia tốc
trọng trường
0 0 0
x y z
g g g
. Khi vật thể B chuyển
động thì hệ tọa độ B không trùng với hệ tọa độ
chuẩn A và khi đó các từ kế và các gia tốc kế hiển
nhiên sẽ cho các chỉ số như sau:

0 0 0 0 0 0
( ) , ( )
T T T T
x y z x y z
B C B B B G C g g g 
(12)
Đặt
1 11 2 12 3 13 4 21 5 22 6 23 7 31 8 32 9 33 10
11 12 13 1 14 2 15 3 1 2 15
, , , , , , , , , ,
, , , , , ( )
x
T
y z
x c x c x c x c x c x c x c x c x c x
x x x b x b x b X x x x

 
         
     

Từ hệ phương trình (6) có các phương trình động lực đối với 9 trạng thái của hệ 15 trạng thái như sau:
1 11 2 12 3 13 4 21 5 22 6 23 7 31 8 32 9 33 10
11 12 13 1 14 2 15 3 1 2 15
, , , , , , , , , ,
, , , , , ( )
x
T
y z
x c x c x c x c x c x c x c x c x c x

x x x b x b x b X x x x

 
         
     
(13)
Từ hệ (13) có dạng rời rạc đối với 9 biến số:


 
 
1 1 1 1 2 12 3 11
2 2 1 2 3 10 1 12
3 3 1 3 1 11 2 10
4 4 1 4 5 12
( ) ( ) ( 1) ( 1) ( 1) ( 1) ( 1)
( ) ( ) ( 1) ( 1) ( 1) ( 1) ( 1)
( ) ( ) ( 1) ( 1) ( 1) ( 1) ( 1)
( ) ( ) ( 1) ( 1) (
k
k
k
k
x k f X x k T x k x k x k x k
x k f X x k T x k x k x k x k
x k f X x k T x k x k x k x k
x k f X x k T x k x





        
        
        
    








6 11
5 5 1 5 6 10 4 12
6 6 1 6 4 11 5 10
7 7 1 7 8 12 9 11
8 8 1
1) ( 1) ( 1)
( ) ( ) ( 1) ( 1) ( 1) ( 1) ( 1)
( ) ( ) ( 1) ( 1) ( 1) ( 1) ( 1)
( ) ( ) ( 1) ( 1) ( 1) ( 1) ( 1)
( ) ( )
k
k
k
k
k x k x k
x k f X x k T x k x k x k x k
x k f X x k T x k x k x k x k

x k f X x k T x k x k x k x k
x k f X




   
        
        
        
 




8 9 10 7 12
9 9 1 9 7 11 8 10
( 1) ( 1) ( 1) ( 1) ( 1)
( ) ( ) ( 1) ( 1) ( 1) ( 1) ( 1)
k
x k T x k x k x k x k
x k f X x k T x k x k x k x k

      
        
(14)


ở đây T- là bước tính khi rời rạc hóa, trong
trường hợp tốc độ góc

zyx

,, biến đối chậm
có coi:

10 10 1 10
11 11 1 11
12 10 1 12
( ) ( ) ( 1)
( ) ( ) ( 1)
( ) ( ) ( 1)
k
k
k
x k f X x k
x k f X x k
x k f X x k



  
  
  
(15)
Độ trôi b
1,
b
2
, b
3

cũng có thể viết dưới dạng:

13 13 1 13
14 14 1 14
15 15 1 15
( ) ( ) ( 1)
( ) ( ) ( 1)
( ) ( ) ( 1)
k
k
k
x k f X x k
x k f X x k
x k f X x k



  
  
  
(16)
Từ biểu thức (12) có thể nhận được hệ 6
phương trình quan sát của véc tơ quan sát
1 2 9
( )
T
Z z z z
 như sau:
1 1 0 11 0 21 0 31 0 1 0 4 0 7
2 2 0 12 0 22 0 32 0 2 0 5 0 8

3 3 0 13 0 23 0 33 0 3 0 6 0 9
4 4 0 11 0 21 0 31 0 1 0 4 0 7
5 5
( )
( )
( )
( )
(
k x y z x y z
k x y z x y z
k x y z x y z
k x y z x y z
k
z h X B c B c B c B x B x B x
z h X B c B c B c B x B x B x
z h X B c B c B c B x B x B x
z h X g c g c g c g x g x g x
z h X
      
      
      
      

0 12 0 22 0 32 0 2 0 5 0 8
6 6 0 13 0 23 0 33 0 3 0 6 0 9
)
( )
x y z x y z
k x y z x y z
g c g c g c g x g x g x

z h X g c g c g c g x g x g x
     
      
(17)
Từ 3 tín hiệu của 3 con quay cho 3 phương trình
quan sát còn lại:
X

Z

Y

x


y


z


G
y

G
x

G
z

T
y

T
x

T
z

492 Trần Đức Thuận, Bùi Hồng Huế, Trương Duy Trung, Trần Xuân Kiên, Nguyễn Sỹ Long
VCM2012
7 7 0 1 10 13
8 8 0 2 11 14
9 9 0 3 12 15
( )
( )
( )
k x
k y
k z
z h X b x x
z h X b x x
z h X b x x



    
    
    
(18)

Thực hiện các phép lấy đạo hàm riêng các hàm
1
( ),( 1,15)
i k
f X i

 theo các biến số x
j
nhận được
ma trận

như sau:
12 11 3 2
12 10 3 1
11 10 2 1
12 11 6 5
12 10 6 4
11 10 5 4
12 11 9 8
1
1 0 0 0 0 0 0 0 0 0 0
1 0 0 0 0 0 0 0 0 0 0
10 0 0 0 0 0 0 0 0 0
0 0 01 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0 0 0
0 0 0 0 0 0 1 0 0 0 0
0 0 0 0 0 0
Tx Tx Tx Tx
Tx Tx Tx Tx

Tx Tx Tx Tx
Tx Tx Tx Tx
Tx Tx Tx Tx
Tx Tx Tx Tx
Tx Tx Tx Tx
Tx
 
 
 
 

 
 
 
2 10 9 7
11 10 8 1
1 0 0 0 0
0 0 0 0 0 0 1 0 0 0 0
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1
Tx Tx Tx
Tx Tx Tx Tx
 
 
 
 

 
 
 
 
 
 
 
 

 
 
 
 
 
 
 
 
 
 
 
 
 
 

Thực hiện các phép lấy đạo hàm riêng các hàm
1
( ),( 1,9)
i k
h X i


 theo các biến số x
j
nhận được
ma trận
H
như sau:
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 1 0 0 1 0 0
0 0 0 0 0 0 0 0 0 0 1 0 0 1 0
0 0 0 0 0 0 0 0 0 0 0 1 0 0 1
x y z
x y z
x y z
x y z
x y z
x y z
H H H
H H H
H H H

g g g
H g g g
g g g
 
 
 
 
 
 
 

 
 
 
 



 



(20)
Khi các nhiễu đo của các từ kế và các gia tốc kế
không tương quan lẫn nhau thì ma trận R sẽ là ma
trận đường chéo (các phần tử ngoài đường chéo
đều bằng 0). Các phần tử của đường chéo chính là
phương sai của nhiễu đo từng từ kế và từng gia tốc
kế và nhiễu đo của các con quay vi cơ đo vận tốc
góc. Ma trận Q cùng là ma trận đường chéo với

các phần tử ở đường chéo được chọn hợp lý tùy
thuộc vào phương tiện tính toán (sai số tính toán).
Như vậy từ thông tin của 3 từ kế, 3 gia tốc kế và
3 con quay vi cơ đo tốc độ góc cho ta véc tơ hàm
quan sát Z (hệ phương trình 17, 18) và từ hệ động
học (14, 15, 16) cho phép sử thuật toán lọc
Kalman (11) để nhận được 9 phần tử của ma trận
cosin chỉ phương C và từ đó xác định 3 góc định
hướng
, ,
  
.
4. Tiến hành mô phỏng kiểm nghiệm thuật
toán
Để tiến hành mô phỏng kiểm nghiệm tính đúng
đắn của thuật toán có thể dùng các công cụ mô
phỏng sau Matlab hoặc Simulink. Các bước mô
phỏng được thực hiện gồm hai giai đoạn:
A.Giai đoạn xác định tham số định hướng thực.
Trong giai đoạn này giải trực tiếp hệ phương trình
(6) với hàm tốc độ góc giả định không có nhiễu và
trôi để tạo véc tơ quan sát (mô phỏng từ kế và gia
tốc kế). Giai đoạn này gồm các bước sau:
- Đặt các điều kiện ban đầu cho các phần tử
ij
c

của hệ phương trình (6).
- Xây dựng chương trình thay đổi các tốc độ
quay

0 0 0
, ,
x y z
  
trong hệ phương trình (7).
- Tiến hành giải hệ phương trình vi phân (6) với
điều kiện ban đầu đã chọn và chương trình
0 0 0
, ,
x y z
  
để xác định ma trận C.
- Đặt các điều kiện ban đầu về từ trường và gia
tốc trọng trường cho hệ tọa độ chuẩn, tức là đặt
các giá trị
0 0 0
x y z
B B B
và
0 0 0
x y z
g g g
và xác định
véctơ B và véctơ G.
B. Giai đoạn thực hiện thuật toán lọc Kalman.
Việc thực hiện thuật toán locjc Kalman theo thứ tự
ở các biểu thức (11) để xác định các phần tử của
ma trận cosin chỉ phương C cần hai chú ý sau:
+ Trên cơ sở matrận cosin chỉ phương nhận
được ở giai đoạn A xây dựng véc tơ quan sát Z

k

như sau:

1 2 9
( , , , )
T
Z z z z
 , trong đó:
1 2 3 4 5 6
( , , ) wt, ( , , ) wg
T T T T
z z z C B z z z C G   
và

0
7 1
0
8 2
0
9 0 2
x x
y y
z z
z b w
z b w
z b w




  
  
  

ở đây b
1,
b
2
, b
3
được đặt tùy ý xong phải thỏa mãn
điều kiện:

1 2 3
( ), ( ), ( )
x y z
b max b max b max
  
  
Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 493
Mã bài: 115
wt và wg
là véc tơ nhiễu ngẫu nhiên
và
, ,
x y z
w w w
là hàm ngẫu nhiên được lấy từ
chương trình tạo hàm ngẫu nhiên chuẩn của hệ
ngôn ngữ mô phỏng với kỳ vọng toán học bằng 0.

+Véc tơ hàm quan sát đánh giá
ˆ ˆ
( ( ))
k k k
Z h X
 
được xác định theo biểu thức:
1 2 3
( , , )
T T
z z z C B
 ,
4 5 6
( , , )
T T
z z z C G
 với điều
kiện ma trận C được hình thành của kết quả ước
lượng Kalman ở bước trước. Còn
7 8 9
, ,
z z z
được
xác định như sau:

7 10 13 8 11 14 7 12 15
, ,
z x x z x x z x x
     
.

C.Tiến hành so sánh các hàm nhận được của
véctơ đánh giá X với các phần tử của ma trận C
nhận được ở giai đoan A như sau:
+
1
( )
x t
so với
11
( )
c t
;
2
( )
x t
so với
12
( )
c t
;
3
( )
x t
so với
13
( )
c t
;
4
( )

x t
so với
21
( )
c t
;
+
5
( )
x t
so với
22
( )
c t
;
6
( )
x t
so với
23
( )
c t
;
7
( )
x t
so với
31
( )
c t

;
8
( )
x t
so với
32
( )
c t
;
+
9
( )
x t
so với
33
( )
c t
;
Nếu các hàm này trùng hoặc gần nhau có thể
đánh gia thuật toán được lập có tính đúng đắn.
Trên các hình 4 là đồ thị quá trình mô phỏng kiểm
nghiệm bằng ngôn ngữ mô phỏng Matlab với bước
tính T=0.005 giây. Qua mô phỏng cho kết quả
khẳng định tính đúng đắn của thuật toán.
11
c
,
21
c
- hệ số lý tưởng. X1, X4-hệ số đánh giá

của bộ lọc Kalman.
0 500 1000 1500 2000 2500 3000 3500 4000 4500 5000
-0.5
0
0.5
1
1.5
C11
Buoc tinh
0 500 1000 1500 2000 2500 3000 3500 4000 4500 5000
-0.5
0
0.5
1
1.5
X1
Buoc tinh

a. Đồ thị hệ số C11 và X1
0 500 1000 1500 2000 2500 3000 3500 4000 4500 5000
-0.5
0
0.5
1
1.5
Buoc tinh
C21
0 500 1000 1500 2000 2500 3000 3500 4000 4500 5000
-0.5
0

0.5
1
1.5
Buoc tinh
X4

b. Đồ thị hệ số C21 và X4
H. 4 Đồ thị các hệ số của ma trận cosin chỉ phương
494 Trần Đức Thuận, Bùi Hồng Huế, Trương Duy Trung, Trần Xuân Kiên, Nguyễn Sỹ Long
VCM2012
5. Kết luận
Việc ứng dụng bộ lọc Kalman phi tuyến mở
rộng cho phép thiết lập thuật toán xác định các
tham số định hướng trong điều kiện con quay vi cơ
đo tốc độ góc có độ trôi và nhiễu trên cơ sở thông
tin quan sát được của từ các từ kế, gia tốc kế và
các con quay vi cơ đặt trên thiết bị di động. Tuy
nhiên, thuật toán này chỉ có giá trị với giả thiết
tổng véctơ các chỉ số của 3 gia tốc kế phải có giá
trị gần bằng gia tốc trọng trường nơi vật thể đang
có mặt và véctơ từ trường trái đất nơi vật thể đang
hoạt động không thay đổi. Những giả thiết này
đúng và gần đúng cho các vật thể như tay máy,
người máy, máy bay không người lái, phương tiện
ngầm hoạt động cự ly ngắn. Việc vật thể chuyển
động với vận tốc gần đều hoặc đứng yên thì lực
ngoài cân bằng với lực trọng trường, khi đó chỉ số
các gia tốc kế sẻ tuân thủ công thức thứ 2 của biểu
thức 12, còn khi vật thể chuyển động có gia tốc
công thức này không còn giá trị. Việc vật thể hoạt

động trong cự ly ngắn thì các giá trị
0 0 0
, ,
x y z
B B B

trong công thức thứ nhất của biểu thức 12 sẽ là
hằng số. Đối với UAV (máy bay không người lái)
hoạt động cự ly xa cần phải có bản đồ từ trường và
bản đồ gia tốc trọng trường đi kèm trong bộ nhớ
của máy tính trên UAV. Đối với tên lửa chuyển
động có gia tốc, thuật toán sẽ khác và sẽ được
trình bày ở công trình khác. Điểm mới của bài báo
là dùng quan sát chỉ số từ kế, gia tốc kế và con
quay hai bậc tự do với công cụ bộ lọc Kalman phi
tuyến mở rộng để xác định các tham số định
hướng với độ chính xác cao.

Tài liệu tham khảo
1. Salychev O.S (1998) “Inertial Systems in
Navigation and Geophysics”, Bauman MSTU Press,
Moscow.
2. Распопов В Я Микросистемная авионика,
учебное пособие .
Тула "Гриф и к" 2010.
3. Фильтрация и стохастическое управление в
динамических системах. Под редакциёй К Т
Леондеса, перевод с английского. Издательство
"Мир" Москва 1980.