CHƯƠNG 2 BÀI THÍ NGHIỆM ĐO LƯỜNG QUÁN TÍNH IMU
2.1.2.Bộ lọc Kalman EKF
2.1.2.1 Giới thiệu
Lọc Kalman là một thuật tốn cung cấp ước tính của một số biến chưa biết dựa trên các phép đo được quan sát theo thời gian. Bộ lọc Kalman đã và đang chứng minh tính hữu ích của nó trong các ứng dụng khác nhau. Bộ lọc Kalman có hình thức tương đối đơn giản và u cầu sức mạnh tính tốn nhỏ. Tuy nhiên, những người không quen thuộc với lý thuyết ước lượng để hiểu và thực hiện các bộ lọc Kalman vẫn không dễ dàng. Trong khi tồn tại một số tài liệu xuất sắc như giải quyết vấn đề đạo hàm và lý thuyết đằng sau bộ lọc Kalman, chương này tập trung vào một quan điểm thực tế hơn.
Hai chương sau sẽ lần lượt giới thiệu các thuật toán của bộ lọc Kalman và bộ lọc Kalman mở rộng, bao gồm các ứng dụng của chúng. Với các mơ hình tuyến tính có nhiễu Gaussian cộng thêm, bộ lọc Kalman cung cấp các ước tính tối ưu. Điều hướng bằng hệ thống vệ tinh dẫn đường toàn cầu (GNSS) sẽ được cung cấp như một ví dụ triển khai
42
của bộ lọc Kalman. Bộ lọc Kalman mở rộng được sử dụng cho các vấn đề phi tuyến như theo dõi mục tiêu theo góc mang và điều hướng tham chiếu địa hình (TRN). Cách thực hiện các thuật tốn lọc cho các ứng dụng như vậy sẽ được trình bày chi tiết.
2.1.2.2 Bộ lọc Kalman
Định nghĩa vấn đề
Bộ lọc Kalman được sử dụng để ước tính trạng thái dựa trên hệ thống động lực học tuyến tính ở định dạng khơng gian trạng thái. Mơ hình q trình xác định sự phát triển của trạng thái từ thời gian k−1 đến thời gian k:
(1)
Trong đó:
• F là ma trận chuyển đổi trạng thái được áp dụng cho vectơ trạng thái trước
đó xk−1
• B là ma trận đầu vào điều khiển được áp dụng cho vectơ điều khiển
• là vectơ nhiễu quá trình được giả định là Gaussian trung bình bằng 0 với hiệp phương sai Q,
Mơ hình q trình được ghép nối với mơ hình đo lường mơ tả mối quan hệ giữa trạng thái và phép đo ở bước thời gian hiện tại k như:
(2)
Trong đó:
• là vectơ đo lường,
• H là ma trận đo lường, và
• là véc tơ tiếng ồn đo lường được giả định là Gaussian trung bình bằng 0 với
43
Lưu ý rằng đôi khi thuật ngữ "đo lường" được gọi là "quan sát" trong các tài liệu khác nhau.
Vai trị của bộ lọc Kalman là cung cấp ước tính về ở thời điểm k , với ước tính ban đầu là x0, một loạt các phép đo
1, ,...,2 k
z z z và thông tin của hệ thống được mô tả bởi F, B, H, Q và R . Lưu ý rằng các chỉ số con của các ma trận này được bỏ qua ở đây bằng cách giả định rằng chúng bất biến theo thời gian như trong hầu hết các ứng dụng. Mặc dù ma trận hiệp phương sai được cho là phản ánh số liệu thống kê của tiếng ồn, nhưng số liệu thống kê thực sự của tiếng ồn vẫn chưa được biết đến hay không Gaussian trong nhiều ứng dụng thực tế. Vì thế, Q và R thường được sử dụng làm thơng số điều chỉnh mà người dùng có thể điều chỉnh để có được hiệu suất mong muốn.
Thuật toán lọc Kalman
Thuật toán lọc Kalman bao gồm hai giai đoạn: dự đoán và cập nhật. Lưu ý rằng các thuật ngữ “dự đoán” và “cập nhật” thường được gọi là “lan truyền” and “chỉnh sửa”, tương ứng, trong các tài liệu khác nhau. Thuật tốn lọc Kalman được tóm tắt như sau:
Dự đốn trạng thái ước tính
Hiệp phương sai lỗi dự đốn
Bảng 2-1: Sự dự đoán thuật toán lọc Kalman
Số dư đo lường (adsbygoogle = window.adsbygoogle || []).push({});
Kalman đạt được
Đã cập nhật ước tính trạng thái Đã cập nhật hiệp phương sai lỗi
44
Trong các phương trình trên, tốn tử mũ, ^, có nghĩa là ước tính của một biến. Đó là, xˆ
là một ước tính của x. Các chỉ số trên - và + biểu thị các ước tính được dự đốn (trước) và được cập nhật (sau), tương ứng.
Ước tính trạng thái dự đốn được phát triển từ ước tính trạng thái được cập nhật trước đó. Thuật ngữ mới P được gọi là hiệp phương sai lỗi trạng thái. Nó mã hóa hiệp phương sai lỗi mà bộ lọc cho rằng lỗi ước tính có. Lưu ý rằng hiệp phương sai của một biến ngẫu
nhiên x được định nghĩa là
Ở đây biểu thị giá trị dự kiến (trung bình) của đối số của nó. Người ta có thể quan sát thấy rằng hiệp phương sai trở nên lớn hơn ở giai đoạn dự đốn do tổng kết với Q, có nghĩa là bộ lọc khơng chắc chắn hơn về ước tính trạng thái sau bước dự đốn.
Trong giai đoạn cập nhật, phần dư đo được tính trước. Phần dư của phép đo, còn được gọi là sự đổi mới, là sự khác biệt giữa phép đo thực, zk, và phép đo ước tính,
. Bộ lọc ước tính số đo hiện tại bằng cách nhân trạng thái dự đoán với ma trận đo lường. Phần dư, , sau đó được nhân với mức tăng Kalman, , để sửa chữa, , với ước tính dự đốn . Sau khi nhận được ước tính trạng thái được cập nhật, bộ lọc Kalman sẽ tính tốn hiệp phương sai được cập nhật, , sẽ được sử dụng trong bước thời gian tiếp theo. Lưu ý rằng hiệp phương sai lỗi được cập nhật nhỏ hơn hiệp phương sai lỗi được dự đốn, có nghĩa là bộ lọc chắc chắn hơn về ước tính trạng thái sau khi phép đo được sử dụng trong giai đoạn cập nhật.
Chúng ta cần một giai đoạn khởi tạo để triển khai bộ lọc Kalman. Là các giá trị ban đầu, chúng tơi cần phỏng đốn ban đầu về ước tính trạng thái, và phỏng đốn ban đầu của ma trận hiệp phương sai, . Cùng với Q và R, và đóng một vai trò quan trọng để đạt được hiệu suất mong muốn. Có một quy tắc chung được gọi là "sự thiếu hiểu biết ban đầu", có nghĩa là người dùng nên chọn một lớn để hội tụ nhanh hơn. Cuối cùng, người ta có thể có được triển khai bộ lọc Kalman bằng cách triển khai
45
các giai đoạn dự đoán và cập nhật cho mỗi bước thời gian, k =1, 2, 3 , …, sau khi khởi tạo các ước tính.
Lưu ý rằng các bộ lọc Kalman được suy ra dựa trên giả định rằng q trình và mơ hình đo lường là tuyến tính, tức là, chúng có thể được biểu thị bằng các ma trận F , B và H , và nhiễu của quá trình và phép đo là Gaussian bổ sung. Do đó, bộ lọc Kalman chỉ cung cấp ước tính tối ưu nếu các giả định được thỏa mãn.
2.1.2.3 Bộ lộc Kalman mở rộng
Định nghĩa vấn đề
Giả sử bạn có một hệ thống động phi tuyến trong đó bạn khơng thể xác định mơ hình q trình hoặc mơ hình đo lường với phép nhân vectơ và ma trận như trong (1) và (2) . Bộ lọc Kalman mở rộng cung cấp cho chúng ta một công cụ để xử lý các mơ hình phi tuyến như vậy một cách hiệu quả. Vì nó rẻ hơn về mặt tính tốn so với các phương pháp lọc phi tuyến khác như bộ lọc khối điểm và bộ lọc hạt, bộ lọc Kalman mở rộng đã được sử dụng trong các ứng dụng thời gian thực khác nhau như hệ thống định vị.
Bộ lọc Kalman mở rộng có thể được xem như một phiên bản phi tuyến của bộ lọc Kalman đã tuyến tính hóa các mơ hình về một ước tính hiện tại. Giả sử chúng ta có các mơ hình sau để chuyển đổi trạng thái và đo lường.
(3)
(4)
Trong đó:
• là chức năng của trạng thái trước đó,
• và đầu vào điều khiển, , cung cấp trạng thái hiện tại .
• là chức năng đo lường liên quan đến trạng thái hiện tại, , để đo lường .
• và là nhiễu Gaussian đối với mơ hình q trình và mơ hình đo lường với hiệp phương sai Q và R , tương ứng.
46
Thuật toán bộ lọc Kalman mở rộng
Tất cả những gì bạn cần là lấy ma trận Jacobian, đạo hàm riêng bậc nhất của một hàm vectơ đối với một vectơ, của mỗi mơ hình trong mỗi bước thời gian như sau:
Lưu ý các đăng ký của FF và HH được duy trì ở đây vì các ma trận thường thay đổi với các giá trị khác nhau của vector trạng thái cho mỗi bước thời gian. Bằng cách này, bạn tuyến tính hóa các mơ hình về ước tính hiện tại. Thuật tốn bộ lọc rất giống với bộ lọc Kalman.
(5) (adsbygoogle = window.adsbygoogle || []).push({});
(6)
Dự đốn trạng thái ước tính
Hiệp phương sai lỗi dự đốn
Bảng 2-3: Sự dự đoán của thuật toán bộ lọc Kalman mở rộng
Số dư đo lường Kalman đạt được
Đã cập nhật ước tính trạng thái Đã cập nhật hiệp phương sai lỗi
Bảng 2-4: Cập nhật thuật toán bộ lọc Kalman mở rộng
Như trong thuật toán bộ lọc Kalman, toán tử mũ, ^ , có nghĩa là ước tính của một biến. Đó là, là một ước tính của . Các chỉ số trên - và + biểu thị các ước tính được dự đốn (trước) và được cập nhật (sau), tương ứng. Sự khác biệt chính so với bộ lọc
47
Kalman là bộ lọc Kalman mở rộng thu được ước tính trạng thái được dự đoán và phép
đo được dự đoán bằng các hàm phi tuyến và , tương ứng.