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: txkien2003@gmail.com; tdtrung_tdh05@yahoo.com 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. . Thuận, Bùi Hồng Huế, Trương Duy Trung, Trần Xuân Kiên, Nguyễn Sỹ Long VCM2 012 Ứ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. 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. Bùi Hồng Huế, Trương Duy Trung, Trần Xuân Kiên, Nguyễn Sỹ Long VCM2 012 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