Đồ án Áp dụng bộ lọc Kalman (hai bộ lọc Kalman) vào bài toán dẫn đường

i Định vị Mạng tế bào và Nhắn tin Khẩn truyền đạt thông tin về vị trí và trạng thái của các phương tiện xe cộ do GPS xác định, rất phù hợp để sử dụng trong các hệ thống nhằm trợ giúp cho các nhà quản lý đường bộ, các hãng giám sát nội vụ, các công ty điện thoại di động, các công ty cho thuê xe ô tô, các nhà khai thác đội tầu biển thương mại và các nhà sản xuất ô tô... tìm kiếm những lợi thế cạnh tranh.. 1.1.5. Mã trong hệ thống định vị toàn cầu. Mỗi một vệ tinh trong hệ thống GPS đều có đồng hồ nguyên tử độ chính xác rất cao để làm cơ sở cho thiết bị phát tần số chuẩn 10,23Mz. Tần số này điều biến 2 sóng mang L1 = 1575,42MHz và L2 = 1227,60MHz. Các sóng mang L1, L2 được điều biến bởi 3 loại mã sau: Mã P: là mã chính xác, có tần số 10,23 MHz, độ dài toàn phần 267 ngày. Tuy vậy người ta đã chia mã này thành các đoạn có độ dài 7 ngày và gắn cho mỗi vệ tinh trong hệ thống GPS một trong các đoạn mã như thế, cứ sau 1 tuần lại thay đổi nên khó bị giải mã để sử dụng nếu ko được phép. Mã P điều biến cả 2 sóng mang L1 và L2. Mã C/A có tần số 1,023MHz, nó chỉ điều biến sóng mang L1, mã C/A được sử dụng cho mục đích dân sự, mỗi vệ tinh được gán 1 mã C/A riêng biệt. Mã D là mã dùng để truyền lịch vệ tinh mới nhất, thông số của lớp khí quyển sóng điện từ truyền qua, thời gian của hệ thống, sai số đồng hồ vệ tinh, phân bố của các vệ tinh trên quỹ đạo... Nó điều biến cả 2 sóng mang L1 và L2. Hoạt động Tần số L1 chứa đựng 2 tín hiệu số, được gọi là mã P và mã C/A. Mã P nhằm bảo vệ thông tin khỏi những sự truy nhập trái phép. Tuy nhiên, mục đích chính của các tín hiệu mã hóa là nhằm tính toán thời gian cần thiết để thông tin truyền từ vệ tinh tới một thiết bị thu nhận trên mặt đất. Sau đó, khoảng cách giữa 2 bên được tính bằng cách nhân thời gian cần thiết để tín hiệu đến nơi với tốc độ của ánh sáng là 300.000 km/giây. Tuy nhiên, tín hiệu có thể bị sai đôi chút khi đi qua bầu khí quyển. Vì vậy, kèm theo thông điệp gửi tới các thiết bị nhận, các vệ tinh thường gửi kèm luôn thông tin về quỹ đạo và thời gian. Việc sử dụng đồng hồ nguyên tử sẽ đảm bảo chính xác về sự thống nhất thời gian giữa các thiết bị thu và phát. Để biết vị trí chính xác của các vệ tinh, thiết bị thu GPS còn nhận thêm 2 loại dữ liệu mã hóa: Dữ liệu Almanac: được cập nhật định kỳ và cho biết vị trí gần đúng của các vệ tinh trên quỹ đạo. Nó truyền đi liên tục và được lưu trữ trong bộ nhớ của thiết bị thu nhận khi các vệ tinh di chuyển quanh quỹ đạo. Dữ liệu Ephemeris: phần lớn các vệ tinh có thể hơi di chuyển ra khỏi quỹ đạo chính của chúng. Sự thay đổi này được ghi nhận bởi các trạm kiểm soát mặt đất. Việc sửa chữa những sai số này là rất quan trọng và được đảm nhiệm bởi trạm chủ trên mặt đất trước khi thông báo lại cho các vệ tinh biết vị trí mới của chúng. Thông tin được sửa chữa này được gọi là dữ liệu Ephemeris. Kết hợp dữ liệu Almanac và Ephemeris, các thiết bị nhận GPS biết chính xác vị trí của mỗi vệ tinh. Nguyên nhân sai số Sai số của phương pháp đinh vị GPS chủ yếu là do 6 nguyên nhân dưới đây (không kể sai số nhân tạo SA đã được cựu tổng thống Bill Clinton ra lệnh tắt): Dữ liệu Ephemeris. Đồng hồ vệ tinh. Trễ ở tầng điện ly. Trễ ở tầng đối lưu. Nhiễu đa đường. Máy thu (bao gồm cả phần mềm). Lỗi dữ liệu Ephemeris xảy ra khi thông điệp của GPS không truyền chính xác vị trí của vệ tinh và vì thế ảnh hưởng tới độ chính xác khi xác định khoảng cách. Lỗi này sẽ tăng theo thời gian từ lần cập nhật cuối cùng của trạm điều khiển. Lỗi do đồng hồ vệ tinh ảnh hưởng tới cả những người dùng mã C/A hay mã P, lỗi này gây ra sai số 1 – 2 mét sau khi cập nhật 12 giờ. Lỗi đo lường gây sai số khoảng cách cỡ vài mét.Trễ ở tầng điện ly và đối lưu gây nên trễ pha khi tính toán khoảng cách (pseudorange). Lỗi này có thể loại trừ khi sử dụng các máy thu dùng mã P có 2 băng tần. Với L1 và L2 có tần số khác nhau, tầng điện ly sẽ làm chúng có độ trễ khác nhau. Đó là cơ sở cho phương pháp loại trừ sai số này. Lỗi đa đường gây ra bởi các tín hiệu bị phản xạ qua các chướng ngại khác nhau tới máy thu. Hiện tượng này sẽ trầm trọng hơn nếu có nhiều chướng ngại và lỗi khoảng cách gây ra có thể tới 15 mét. Vai trò chính của GPS đó là cung cấp chính xác các thông số vị trí và vận tốc của vật thể bay. GPS có thể được sử dụng để hỗ trợ cho các hệ thống dẫn đường khác mà tiêu biểu là sự kết hơp GPS / INS. Hệ thống dẫn đường quán tính (INS). Một số khái niệm cơ bản sau : Quán tính: là bản chất của vật thể mà khi không có lực tác động thì nó sẽ chuyển động tịnh tiến đều hoặc chuyển động vòng tròn đều. Hệ quy chiếu quán tính: hệ quy chiếu mà ba định luật Newton được áp dụng và bảo toàn. Cảm biến quán tính: gồm 2 loại là gia tốc kế và cảm biến vận tốc góc (còn gọi con quay vi cơ). Hệ thống dẫn đường quán tính: là hệ thống sử dụng các cảm biến vận tốc góc và cảm biến gia tốc để ước lượng vị trí, vận tốc, độ cao và vận tốc thay đổi độ cao của vật thể bay. Góc hướng Z Góc trúc Y Góc nghiêng X khối tâm 0 Hình 1.4: Trục toạ độ của hệ thống dẫn đường quán tính Hệ thống INS gồm ba cảm biến vận tốc góc cho phép xác định vận tốc góc nghiêng, góc chúc và góc hướng trong hệ toạ độ vật thể bay (xem hình 1.4). Hệ thống INS cũng có thể có thêm ba cảm biến gia tốc cho phép xác định gia tốc theo ba trục của hệ toạ độ vật thể bay này. Hiện nay có hai cấu trúc dẫn đường quán tính tiêu biểu nhằm xác định các góc Ơle từ các cảm biến vận tốc góc là cấu trúc gắn chặt (gimble) và cấu trúc nổi (strapdown). Cấu trúc strapdown hiện được sử dụng rộng rãi hơn, trong đó các cảm biến gia tốc và vận tốc góc được gắn chặt vào vật thể bay. Các giá trị gia tốc thu được từ các cảm biến gia tốc được hiệu chỉnh với vận tốc quay của trái đất và gia tốc trọng trường nhằm xác định vị trí và vận tốc chính xác của vật thể bay. Phương trình động học. Định hướng của vật thể bay với hệ trục quả đất cố định được xác định bởi ba góc Ơle (hình 1.5). Phương trình (1.1) biểu diễn sự liên hệ giữa các vận tốc góc nghiêng, vận tốc góc chúc và vận tốc góc hướng ( ký hiệu là p,q và r) và 3 góc Ơle: (1.1) Tích phân phương trình (1.1) ta sẽ thu được 3 góc Ơle. Hình 1.5. Ba góc Ơle Các gia tốc ax, ay và az của vật thể bay dọc theo 3 trục toạ độ vật thể bay liên hệ với vận tốc U, V và W trên hệ trục quả đất cố định (XYZ) theo hệ phương trình (1.2): (1.2) Thực hiện tích phân và sẽ thu được các vận tốc U, V và W. Sau đó đổi hệ trục toạ độ nhờ sử dụng ma trận cosin trực tiếp DCM để thu được vận tốc theo hướng bắc, hướng đông và hướng về trái đất (người ta gọi đó là hệ trục toạ độ dẫn đường, xem hình 1.4). (1.3) Ở đó : Tích phân VN,VE và VD sẽ thu được vị trí của vật thể bay trên bề mặt trái đất (hệ trục toạ độ dẫn đường). Vĩ độ , kinh độ và độ cao H của vật thể bay có thể xác định nhờ hệ phương trình sau: (1.4) Hình 1.6: Hệ trục toạ độ dẫn đường 1.2.2. Ưu điểm và nhược điểm của INS Ưu điểm của hệ INS - Hoạt động tự trị. - Tần số cập nhật cao. - Các sai số có đặc tính thay đổi chậm, ít chịu ảnh hưởng bên ngoài. - Có khả năng đo các góc định hướng. Nhược điểm Có nhiều loại sai số trong các hệ thống INS và chủ yếu là do các cảm biến quán tính gây nên. Dưới đây là một số lỗi gây ra bởi các cảm biến gia tốc và vận tốc góc. - Lổi vị trí khi láp đặt cảm biến gây nên sai số về góc góc nghiêng , góc trúc và góc hướng. - Lỗi về độ lệch của cảm biến gia tốc dẫn lối gia của cảm biến gia tốc sẽ bị lệch đi một giá trị không đổi. Giá trị này lại thay đổi khi bật hoặc tát thiết bị. - Hiện tượng lệch và trôi của cảm biến vận tốc góc ( do tác động của nhiệt độ) gây nên vật không chuyển động nhưng vẫn có vận tốc góc không đổi. - Nhiễu ngẫu nhiên gây nên lổi ngẩu nhiên trong đo lường. Những lỗi trong đo gia tốc và vận tốc góc sẽ dẫn tới các lỗi tăng dần khi xác định vị trí và vận tốc của vật thể bay (do việc lấy tích phân). Các lỗi tăng dần này được gọi là lỗi dẫn đường, bao gồm 9 lỗi (3 lỗi vị trí, 3 lỗi vận tốc, 3 lỗi tư thế). Có thể nhận thấy chắc chắn rằng hệ thống dẫn đường quán tính không thể hoạt động tự trị được mà phải được kết hợp với một hệ thống khác. Chương 2: Lý thuyết hệ thống dẫn đường tích hợp GPS/INS 2.1 Giới thiệu về hệ dẫn đường quán tính. Dẫn đường quán tính dựa trên cơ sở tính toán vị trí, vận tốc và hướng của một vật thể chuyển động bằng các sử dụng các thông số đo được từ các cảm biến như cảm biến gia tốc và cảm biến vận tốc góc. Các cảm biến quán tính và đo chuyển động đều dựa vào các định luật vật lý về tự nhiên và không chịu ảnh hưởng của các tín hiệu điện hay điện từ bên ngoài, điều này là cơ sở cho cảm biến quán tính hoạt động đáng tin cậy và khó bị nhiễu. Cảm biến quán tính hoạt động dự trên định luật II Newton: F (2.1) Với : F là vectơ tổng các lực tác dụng lên vật thể. m là khối lượng vật thể. là đạo hàm bậc nhất quãng đường chuyển động của vật thể (vận tốc chuyển động). Phương trình (2.1) cần được đưa về dạng liên hệ với vectơ gia tốc trọng trường g. = F + mg (2.2) Gia tốc sinh ra do lực F tác dụng lên vật là a = , thay vào phương trình (2.2) ta có: = a + g (2.3) Các phương trình này được ấn định cho hệ tọa độ quán tính Newton. Đối với hệ tọa độ thứ i thì phương trình (2.3) có dạng. = g() + a (2.4) Với là vị trí trong hệ tọa độ thứ i, g là gia tốc trọng trường của hệ tọa độ thứ i phụ thuộc vào vị trí x, a là gia tốc thành phần được đo bởi các cảm biến gia tốc. Từ các phường trình này có thể xác định được vị trí x và vân tốc của vật thể. 2.2 Các hệ tọa độ 2.2.1 Hệ tọa độ quán tính (Inertial Frame). Hệ tọa độ quán tính là một hệ tọa độ tuân theo 3 định luật của Newton. Gốc của hệ tọa độ quán tính và hướng của các trục là tuỳ ý. Để cho thuận lợi, hệ tọa độ quá tính thường được định nghĩa trùng với tâm của trái đất. Hệ tọa độ này cũng được gọi là hệ tọa độ thứ i để tránh nhầm lẫn với hệ tọa độ quán tính lí tưởng. Tất cả các phép đo quán tính đều có quan hệ đến hệ tọa độ này. Các thông số trong hệ tọa độ này sẽ có ký tự biểu tượng thứ i, ví dụ . 2.2.2 Hệ tọa độ cố định tâm trái đất (The Earth Fixed Frame) Hệ tọa độ cố định tâm trái đất (e-frame) có gốc tọa độ là tâm của trái đất. Trục x được định nghĩa có chiều dương quay về phía giao điểm của đường kinh tuyến 0 và đường xích đạo, trục z là trục quay của trái đất có chiều dương hướng lên bắc cực, trục y là tích có hướng của trục z và trục x sao cho hệ tọa độ cố định tâm trái đất là một hệ thuận. Các thông số trong hệ tọa độ này sẽ có ký tự biểu tượng e, ví dụ xe. 2.2.3 Hệ tọa độ định vị (Navigation frame) Hệ tọa độ định vị được sử dụng để mô tả sự chuyển động của vật thể theo các hướng bắc, đông, và hướng đi lên vuông góc bề mặt trái đất-hệ ENU (hình 2.1). Ngoài ra hệ định vị còn có thể định nghĩa trục hướng đi lên thành trục hướng đi xuống, tức là đi thẳng vào tâm trái đất-Hệ NED. Các thông số trong hệ tọa độ định vị sẽ có biểu tượng n, ví dụ xn . Khi chuyển đổi giữa 2 hệ quy chiếu ENU và NED cần phải chú ý đổi các giá trị của x, y và z. Hình 2.1: Hệ tọa độ định vị 2.2.4 Hệ tọa độ gắn liền vật thể (Body frame). Hệ tọa độ gắn liền vật thể lấy vật thể làm gốc hệ tọa độ, từ đó mô tả các chuyển động theo các hướng trước, sau, trái phải, lên, xuống và các góc quay Euler 2.3 Phương trình định vị. Giả sử a là một hệ tọa độ bất kỳ quay quanh hệ tọa độ quán tính i với vận tốc góc là . Mối liên hệ khi chuyển từ hệ tọa độ a sang hệ tọa độ quán tính i là: xi= xa (2.5) Với là vị trí trên hệ tọa độ i, là 1 vectơ 3 thành phần. là vị trí trên hệ tọa độ i, là 1 vectơ 3 thành phần. là ma trận chuyển từ hệ tọa độ a sang hệ tọa độ quán tính i. Lấy đạo hàm theo thời gian của ma trận chuyển đổi ta được: (2.6) Với là ma trận phản đối xứng được tạo bởi các thành phần của = (): (2.7) Ma trận chuyển đổi đạo hàm cấp 2 của hai hệ tọa độ là: (2.8) Đạo hàm phương trình (2.5) sử dụng các phương trình (2.6) và (2.8) ta được: = = (2.9) Giải lấy và kết hợp với (2.4) và sử dụng tính trực giao của ma trận , ta thu được hệ phương trình động học trong hệ tọa độ a: (2.10) Với và . Hệ phương trình (2.10) là ba phương trình vi phân bậc 2, có thể chuyển đổi thành sáu phương trình vi phân bậc 1 như sau: (2.11) Đây chính là hệ phương trình định vị đối với một hệ tọa độ bất kỳ. 2.4 Hệ Phương trình định vị trong hệ tọa độ cố định tâm trái đât (e-frame). Trái đất chỉ quay quanh trục z , do đó trong hệ tọa độ cố định tâm trái đất e-frame thì chỉ có thành phần là khác không, còn các thành phần còn lại đều bằng không. Do là một hằng số nên = 0, và (2.12) với (2.13) với là vận tốc quay của trái đất. được chuyển đổi từ gia tốc mà các cảm biến gia trốc đo được trong hệ tọa độ gắn liền vật thể. (2.14) Ma trận chuyển đổi được xác định bằng cách tính tích phân các vận tốc góc thu được từ các cảm biến vận tốc góc. (2.15) Thành phần có thể được viết dưới dạng của các vận tốc góc đo được từ các cảm biến () như sau: (2.16) Các phương trình từ (2.12) đến (2.16) là các phương trình vi phân bậc nhất mô tả mối liên hệ giữa vị trí, vận tốc và tư thế của vật thể với dữ liệu thu được từ khối IMU là và . Một cách tổng quát có thể hiểu các phương trình vi phân được viết dưới dạng: (2.17) với bao gồm cả a và . 2.5. Tổng quan về hệ cảm nhận quán tính IMU. Những khối IMU thời kì đầu sử dụng những cảm biến quán tính hoạt động theo nguyên tắc cơ khí. Những cảm biến cơ khí này thường có kích thước lớn, hoạt động kém hiệu quả, giá thành cao và tiêu thụ nhiều năng lượng. Ngày nay, cùng với sự tiến bộ của khoa học công nghệ, đặc biệt là công nghệ vật liệu mới và công nghệ vi chế tạo đã tạo ra các cảm biến vi cơ có kích thước rất nhỏ (cỡ centimet), hoạt động hiệu quả, tiêu thụ ít năng lượng và đặc biệt là giá thành hạ, điều này mở ra một khả năng rộng lớn cho việc ứng dụng các cảm biến vi cơ trong nhiều lĩnh vực đời sống. Một khối vi cơ IMU được cấu tạo từ các cảm biến vi cơ, thường là 3 cảm biến gia tốc và 3 cảm biến vận tốc góc, hoặc cũng có thể là 1 cảm biến gia tốc 3 chiều kết hợp với 3 cảm biến vận tốc góc. Các cảm biến vi cơ kết cấu hỗ trợ với nhau theo cấu trúc gắn liền (hình 2.2a) hoặc theo cấu trúc nổi (hình 2.2b), từ đó có thể xác định được 3 thành phần chuyển động quay và tịnh tiến của vật thể. Hình 2.2: Các cấu trúc của khối IMU vi cơ. Điểm khác nhau cơ bản của hai kiểu cấu trúc này đó là: với kiểu gắn liền thì các cảm biến không bị thay đổi hướng theo đối tượng chuyển động, còn trong kiểu Strapdown thì các cảm biến được gắn chặt với vật chuyển động, do đó sẽ thay đổi trang thái chuyển động theo vật đó. Trên thực tế khối IMU có cấu trúc kiểu Strapdown được sử dụng rộng rãi hơn bởi cấu trúc này đơn giản và có giá thành chế tạo thấp với độ chính xác có thể chấp nhận được.Khi kết hợp các cảm biến vi cơ thành một cấu trúc tổng thể thì thường tạo ra sai số. Sai số mắc phải trong việc sử dụng các cảm biến vi cơ này có ở 2 cấp độ, cấp độ cảm biến và cấp độ nhóm cảm biến. Ở cấp độ cảm biến là sai số của từng cảm biến cấu tạo tên khối IMU, còn ở cấp độ nhóm cảm biến là sai số tổ hợp của nhóm cảm biến với nhau . 2.6 Thuật toán dẫn đường quán tính. Thuật toán dẫn đường quán tính sẽ tính toán vận tốc, tư thế vật thể trong hệ tọa độ định vị. Dữ liệu vào là các thông tin ban đầu về vị trí và dữ liệu thu được từ khối IMU . Lưu đồ thuật toán được mô tả trong hình 2.3 và 2.4 với các phương trình chi tiết sau: Các ký hiệu trong lưu đồ: : các vân tốc góc và gia tốc thu được từ các cảm biến. hN1: thời gian lấy tính phân. hN3: bước thời cập nhật của hệ thống INS. là ma trận chuyển từ hệ tọa độ gắn liền vật thể sang hệ tọa độ định vị. là ma trận chuyển từ hệ tọa độ định vị sang hệ tọa độ cố định tâm trái đất là 3 góc quay Roll, Pitch, và Yaw. là vĩ độ, kinh độ và góc phương vị. Vx,y,y là các vận tốc trong hệ tọa độ cố định tâm trái đất. ,là các vận tốc trong hệ tọa độ định vị. Tính Quaternion -1 Tính lại độ tăng vận tốc trong hệ tọa độ định vị hN3 Liên kết Liên kết hN1 Chỉnh lỗi góc (Coning) Bù lỗi vận tốc Sculling hN3 hN1 Bù nhiễu của cảm biến vận tốc góc theo mô hình (2.18) Bù nhiễu của cảm biến gia tốc theo mô hình (2.18) Tính độ tăng về góc Tính độ tăng về vận tốc IMU Hình 2.3 Thuật toán dẫn đường quán tính Tính Quaternion -2 hN3 Chuẩn hoá các tham số quaternion Tính ma trận Tính tọa độ Tính toán tư thế Tính vận tốc Vx,y Tính ma trận hN3 hN3 Hinh2.4: Thuật toán dẩn đường quán tính. 2.7. Các loại nhiễu ảnh hưởng đến khối IMU. Khối IMU chịu ảnh hưởng của hai loại nhiễu là nhiễu tất định và nhiễu thống kê. 2.7.1. Nhiễu tất định Các loại nhiễu tất định của khối IMU gồm có độ lệch vận tốc góc, độ lệch gia tốc, lỗi tỉ lệ vận tốc góc, lỗi tỉ lệ gia tốc, lỗi vị trí. 2.7.2 Nhiễu thống kê Có thể liệt kê các loại nhiễu thống kê chính tác động đến hệ thống dẫn đường quán tính như sau: Ồn lượng tử: sinh ra khi chuyển tín hiệu tương tự sang dạng số. Nguyên nhân của của nó là sự sai khác giữa mức lượng tử số và biên độ thật của tín hiệu tương tự. Có thể hạn chế ồn lượng tử bằng các phương pháp mã hóa, thay đổi tần số lấy mẫu hoặc tăng thêm mức lượng tử. Nhiễu trắng : là nguyên nhân chính gây ra lỗi thống kê của các khôi IMU và có công suất không đổi trên toàn dải tần. Bản chất của các lỗi bước góc ngẫu nhiên (đối với cảm biến vận tốc góc) và bước vận tốc ngẫu nghiên (đối với cảm biến gia tốc) đều xuất phát từ nhiễu trắng. Bước ngẫu nhiên: Đây là loại nhiễu không có nguồn gốc rõ ràng, và có thể thẫy loại nhiễu này tăng theo hàm mũ với khoảng tương quan thời gian dài. Đối với cảm biến vận tốc góc có bước vận tốc góc ngẫu nhiên, đối với cảm biến gia tốc có bước gia tốc ngẫu nhiên. Nhiễu rung :Nhiễu rung là loại nhiễu ở tần số thấp, gây nên sự mất ổn định của độ lệch, nguồn gốc của loại nhiễu này là do các linh kiện điện tử nhạy với rung ngẫu nhiên. Chương 3: Áp dụng bộ lọc Kalman (hai bộ lọc Kalman) vào bài toán dẫn đường 3.1. Bộ lọc Kalman. Năm 1960 R.E Kalman đã xuất bản một bài báo với tiêu đề “A New Approach to Linear Filtering and Predication Problems”. Nghiên cứu của Kalman đã khắc phục hạn chế của bộ lọc Weiner-Hopf trong việc giải quyết bài toán thống kê tự nhiên. Kể từ đó, danh từ bộ lọc Kalman đã ra đời. Bộ lọc này ước lượng trạng thái của quá trình thời gian rời rạc theo phương trình sai phân tuyến tính: (3.1) Với việc đo (3.2) Biến ngẫu nhiên wk và vk biểu diễn nhiễu đo và nhiễu quá trình. Trong thuật toán lọc Kalman, đặc tính thống kê của hai biến này phải được biết trước. Chúng ta giả sử các biến này độc lập có phổ trắng và phân bố Gauss. P(W))~N(0,Q) (3.3) P(R)~N(0,R) Trong thực tế, ma trận hiệp phương sai nhiễu quá trình Q và ma trận hiệp phương sai nhiễu đo R phải thay đổi theo từng thời điểm, tuy nhiên chúng ta có thể giả sử là hằng số. Ma trận A(nxn) trong phương trình sai phân (3.1) là ma trận chuyển trạng thái từ thời điểm trước (k-1) sang thời điểm hiện tại (k). Chú ý rằng, trong thực tế A có thể thay đổi theo từng thời điểm. Nhưng chúng ta cũng có thể giả sử nó là hằng số. Ma trận B (nx1) là ma trận điều khiển có lối vào . Ma trận H(mxn) trong phương trình (3.2) là ma trận đo lường. Trong thực tế H có thể thay đổi theo từng thời điểm, ở đây chúng ta giả sử là hằng số. 3.1.1.Bản chất tính toán của bộ lọc. Chúng ta định nghĩa là trạng thái tiền ước lượng ở thời điểm thứ k, là trạng thái hậu ước lượng tại thời điểm thứ k và cho ra giá trị đo zk. Chúng ta có thể định nghĩa các lỗi tiền ước lượng và lỗi hậu ước lượng như sau: (3.4) (3.5) Ma trận hiệp phương sai lỗi tiền ước lượng: (3.6) Ma trận hiệp phương sai lỗi hậu ước lượng: (3.7) Xuất phát từ phương trình cho bộ lọc Kalman, chúng ta đi tìm ra một phương trình tính toán trạng thái hậu ước lượng như là một tổ hợp tuyến tính của trạng thái tiền ước lượng và sự khác nhau giữa giá trị đo thực tế z k và giá trị tiên đoán được chỉ trong phương trình sau. (3.8) Giá trị trong công thức (3.8) được gọi là giá trị sai khác giữa giá trị tiên đoán và giá trị thực tế z k đo được. Giá trị này bằng 0 nghĩa là hai giá trị hoàn toàn đồng nhất với nhau. Ma trận K (mxn) trong phương trình (3.8) gọi là hệ số khuếch đại Kalman nhằm mục đích tối thiểu hoá hiệp phương sai lỗi hậu ước lượng (3.7). Độ khuếch đại Kalman có thể được xác định bởi phương trình sau: (3.9) Quan sát phương trình (3.9), chúng ta thấy rằng, khi ma trận hiệp phương sai lỗi đo lượng R tiến tới 0 thì hệ số khuếch đại K được xác định như sau: Trường hợp khác, khi hiệp phương sai lỗi tiền ước lượng tiến tới 0 thì: Khi hiệp phương sai lỗi đo lường R tiến đến 0 thì giá trị zk là chính xác hơn, trong khi giá trị tiên đoán lại kém chính xác. Trường hợp, khi giá trị hiệp phương sai lỗi ước lượng trước tiến tới 0, giá trị zk là kém chính xác trong khi đó giá trị tiên đoán lại đạt độ chính xác hơn. 3.1.2. Bản chất thống kê của bộ lọc. Công thức (3.8) thể hiện bản chất thống kê của tiền ước lượng quy định trên tất cả các giá trị đo trước zk (Quy tắc Bayer). Trạng thái hậu ước lượng trong phương trình (3.8) phản ánh giá trị trung bình (mômen bậc 1) của phân bố trạng thái nếu các điều kiện (3.3) được thoả mãn. Hiệp phương sai lỗi trạng thái hậu ước lượng trong công thức (3.6) phản ánh sự thay đổi của phân bố trạng thái (mômen bậc 2). ~ Nói tóm lại, Bộ lọc Kalman ước lượng một quá trình bằng việc sử dụng một dạng của điều khiển phản hồi: bộ lọc ước lượng trạng thái quá trình tại một vài thời điểm và sau đó quan sát phản hồi trong dạng của nhiễu đo. Các phương trình trong bộ lọc Kalman chia thành hai nhóm: các phương trình cập nhật thời gian và các phương trình cập nhật đo. Các phương trình cập nhật thời gian có nhiệm vụ dự đoán trước trạng thái hiện tại và hiệp phương sai lỗi ước lượng cho thời điểm tiếp theo. Các phương trình cập nhật đo có nhiệm vụ phản hồi, ví dụ việc kết hợp chặt chẽ giá trị đo mới vào giá trị tiền ước lượng để thu được những cải tiến trong giá trị hậu ước lượng. Phương trình cập nhật thời gian có thể gọi là phương trình tiên đoán, trong khi phương trình cập nhật đo có thể coi như phương trình của bộ sửa sai. Cập nhật thời gian (tiên đoán) Cập nhật đo (sửa sai) Hình 3.1: Thuật toán lọc Kalman. Các phương trình cập nhật thời gian của bộ lọc Kalman rời rạc Các phương trình cập nhật đo của bộ lọc Kalman rời rạc: Trạng thái ước lượng được cập nhật xk Zk Khởi tạo Tính hệ số khuếch đại Kalman Cập nhật ước lượng Tiên đoán Cập nhật Hiệp phương sai Hình 3.2: Sơ đồ thực hiện thuật toán Kalman. 3.2. Xây dựng bộ lọc Kalman cải tiến. 3.2.1. Hệ thống dẫn đường tích hợp GPS / INS. Hệ thống dẫn đường quán tính INS có 2 ưu điểm nổi bật khi so sánh với các hệ thống dẫn đường khác là khả năng hoạt động tự trị và độ chính xác cao trong những khoảng thời gian ngắn. Lỗi nghiêm trọng nhất của hệ thống dẫn đường quán tính INS là do các cảm biến quán tính gây ra. Chính vì thế trong những ứng dụng thời gian dài thì hệ thống dẫn đường quán tính INS thường sử dụng với các hệ thống hỗ trợ khác như hệ thống dẫn đường vô tuyến (Loran, Omega và Tacan), hệ thống dẫn đường vệ tinh (GPS, GLONASS và Transit), JTIDS, DME…Các hệ thống này hoạt động ổn định theo thời gian và vì thế cần tích hợp INS và các hệ thống hỗ trợ này. Sự kết hợp GPS và INS là lý tưởng nhất vì hai hệ thống này có khả năng bù trừ nhau hiệu quả. Trái tim của hệ thống tích hợp này chính là bộ lọc tối ưu Kalman. Bộ lọc Kalman rất hiệu quả và linh hoạt trong việc kết hợp đầu ra bị nhiễu của cảm biến quán tính để ước lượng trạng thái của hệ thống không ổn định. Tín hiệu bị nhiễu từ các cảm biến quán tính và GPS bao gồm các thông tin về vị trí, vận tốc, toạ độ của vật thể bay. Những tác nhân làm hệ thống không ổn định là nhiễu do cảm biến, do người sử dụng và nhiễu do môi trường. Bộ lọc Kalman dùng để ước lượng lỗi do các cảm biến quán tính gây ra và ta xây dựng được vectơ trạng thái của bộ lọc từ các lỗi này. Các giá trị thu được từ GPS dùng để xây dựng vectơ đo lường z. Sau khi mô hình hoá các lỗi này thì thực hiện chu trình Kalman với các ước lượng vectơ trạng thái và ma trận tương quan tại thời điểm ban đầu. Cấu trúc này được gọi là cấu trúc GPS hỗ trợ INS và các lỗi được xử lý theo kiểu vòng mở và vòng đóng như mô tả trong hình 3.3 và 3.4. Cấu trúc vòng mở cho phép thực thi dễ dàng hơn nhưng cấu trúc vòng kín lại cho kết quả chính xác hơn. Hình 3.3: Cấu trúc GPS/INS vòng mở. Hình 3.4: Cấu trúc GPS/INS vòng kín. 3.2.2. Bộ loc kalman cải tiến. Trong khoá luận này, cấu trúc lọc Kalman được phát triển bao gồm hai bộ lọc song song được sử dụng rất linh hoạt trong hệ thống dẫn đường. Khi tín hiệu GPS thu được tốt thì hệ thống INS/GPS hoạt động với cấu hình vòng kín. Khi tín hiệu GPS bị mất thì hệ thống dễ dàng chuyển sang cấu hình vòng mở. Khi tín hiệu GPS lại được khôi phục thì hệ thống lại trở về cấu hình vòng kín. Việc chuyển linh hoạt giữa hai cấu hình vòng kín và vòng hở giúp nâng cao chất lượng của toàn bộ hệ thống và khắc phục được nhược điểm của cả hai cấu hình riêng lẻ. Hình 3.5: Bộ lọc kalman song song. Bộ lọc Kalman thứ nhất (KF1) được thiết lập với véctơ 3 trạng thái là sai số vận tốc (eVN, eVE, eVD), véctơ đo lường là hiệu giữa vận tốc