Khác với các bộ lọc bù được áp dụng trên miền tần số, bộ lọc Kalman hoạt động trên miền thời gian và quan tâm tới các đặc trưng thống kê của hệ thống thay vì hàm truyền hay miền tần số. Bộ lọc Kalman sử dụng các phương trình toán học để mô tả và ước lượng tối ưu trạng thái của hệ thống, sử dụng hai mô hình: mô hình động học và mô hình đo lường (nhóm nghiên cứu đã có một số công trình về bộ lọc Kalman như liệt kê 11-15 phần Tài liệu tham khảo]
∑ LPF
HPF
Biểu diễn liên tục theo thời gian mô hình động học có dạng [9, tr.25]:
𝑥̇(𝑡) = 𝐹(𝑡)𝑥(𝑡) + 𝐵(𝑡)𝑢(𝑡) + 𝐺(𝑡)𝑣(𝑡) (3.2)
Trong đó x(t) là véc tơ trạng thái của hệ thống, F(t) là ma trận động học, G(t) là ma trận điều khiển đầu vào, u(t) là véc tơ điều khiển đầu vào, G(t) là ma trận điều khiển nhiễu và v(t) là nhiễu quá trình
Và mô hình đo lường: 𝑧(𝑡) = 𝐻(𝑡)𝑥(𝑡) + 𝑤(𝑡) (3.3) Trong đó z(t) là véc tơ đo lường, H(t) là ma trận đo và w(t) là nhiễu đo
Biểu diễn rời rạc theo thời gian, mô hình động học có dạng:
𝑥𝑘 = 𝐹𝑘𝑥𝑘−1+ 𝐵𝑘𝑢𝑘 + 𝐺𝑘𝑣𝑘 (3.4) Trong đó xk là véc tơ trạng thái đặc trưng cho hệ thống ở thời điểm k kích thước Nx1, Fk là ma trận chuyển trạng thái có kích thước NxN xác lập mối liên hệ giữa trạng thái của hệ thống ở thời điểm k và thời điểm k-1 trước đó, uk là véc tơ điều khiển đầu vào kích thước Rx1, Bk là ma trận kích thước NxR thể hiện tác động của đầu vào lên trạng thái của hệ, Gk là ma trận kích thước NxQ thể hiện tác động của nhiễu quá trình lên trạng thái của hệ và vk là véc tơ nhiễu quá trình kích thước Qx1
Mô hình đo lường có dạng: 𝑧𝑘 = 𝐻𝑘𝑥𝑘 + 𝑤𝑘 (3.5) Trong đó zk là véc tơ đo lường có kích thước Mx1, Hk là ma trận kích thước MxN xác lập mối quan hệ giữa véc tơ đo và trạng thái của hệ và wk là véc tơ nhiễu đo kích thước mx1. wk và vk được giả thiết là nhiễu trắng, phân bố chuẩn, không tương quan với nhau:
𝑝(𝑣) ~ 𝑁(0, 𝑄) (3.6)
𝑝(𝑤) ~ 𝑁(0, 𝑅) (3.7)
Q, R tương ứng là các ma trận hiệp phương sai của nhiễu trạng thái và nhiễu đo ở đây được giả thiết là hằng số.
Bộ lọc Kalman hoạt động theo hai bước: bước dự đoán và bước cập nhật. Trong bước dự đoán, bộ lọc Kalman thực hiện dự đoán trạng thái và hiệp phương sai lỗi ở trạng thái k dựa vào các thông tin có được ở trạng thái k-1 trước đó
𝑥̂𝑘|𝑘−1 = 𝐹𝑘𝑥̂𝑘−1|𝑘−1+ 𝐵𝑘𝑢𝑘 (3.8)
Trong đó 𝑥̂𝑘|𝑘−1, 𝑃𝑘|𝑘−1 là ước lượng của trạng thái và hiệp phương sai lỗi của hệ thống tại thời điểm k dựa vào thông tin từ thời điểm k-1, 𝑥̂𝑘−1|𝑘−1, 𝑃𝑘−1|𝑘−1 là ước lượng tốt nhất của trạng thái và hiệp phương sai lỗi tại thời điểm k-1
Trong bước cập nhật trạng thái và hiệp phương sai lỗi tại thời điểm k tiếp tục được dự đoán dựa trên các giá trị được ướng lượng ở bước dự đoán trước đó và thông tin của phép đo zk có được tại thời điểm k. Giả thiết rằng trạng thái ước lượng mới 𝑥̂𝑘|𝑘 là tổ hợp tuyến tính có trọng số của giá trị ước lượng ở bước dự đoán và giá trị đo zk:
𝑥̂𝑘|𝑘 = 𝑊𝑘′𝑥̂𝑘|𝑘−1+ 𝑊𝑘𝑧𝑘 (3.10) Wk’ và Wk là các ma trận trọng số (khuếch đại) thay đổi theo thời gian, được chọn sao cho tối thiểu trung bình bình phương sai số ước lượng.
Sai số ước lượng tại thời điểm k (bước cập nhật):
𝑥̃𝑘|𝑘 = 𝑥̂𝑘|𝑘 − 𝑥𝑘 (3.11)
= 𝑊𝑘′𝑥̂𝑘|𝑘−1 + 𝑊𝑘𝑧𝑘 − 𝑥𝑘 (kết hợp với 3.5 và 3.10)
= 𝑊𝑘′(𝑥̃𝑘|𝑘−1+ 𝑥𝑘) + 𝑊𝑘(𝐻𝑘𝑥𝑘+ 𝑤𝑘) − 𝑥𝑘
= (𝑊𝑘′+ 𝑊𝑘𝐻𝑘 − 1)𝑥𝑘+ 𝑊𝑘′𝑥̃𝑘|𝑘−1+ 𝑊𝑘𝑤𝑘 (3.12) Chúng ta đã biết để trung bình bình phương sai số ước lượng thì trung bình sai số ước lượng cũng phải tối thiểu. Trong khi đó theo giả thiết E(wk) = 0 và E(𝑥̃𝑘|𝑘−1 | 𝑍𝑘−1) = 0, ta sẽ chọn 𝑊𝑘′+ 𝑊𝑘𝐻𝑘 − 1 = 0 để tối thiểu trung bình sai số ước lượng E(𝑥̃𝑘|𝑘 | 𝑧𝑘) Do đó 𝑊𝑘′ = 1 − 𝑊𝑘𝐻𝑘. Thay vào (3.10) và (3.12) ta có:
𝑥̂𝑘|𝑘 = (1 − 𝑊𝑘𝐻𝑘)𝑥̂𝑘|𝑘−1+ 𝑊𝑘𝑧𝑘 = 𝑥̂𝑘|𝑘−1+ 𝑊𝑘(𝑧𝑘− 𝐻𝑘𝑥̂𝑘|𝑘−1) (3.13)
𝑥̃𝑘|𝑘 = 𝑊𝑘′𝑥̃𝑘|𝑘−1+ 𝑊𝑘𝑤𝑘 = (1 − 𝑊𝑘𝐻𝑘)𝑥̃𝑘|𝑘−1+ 𝑊𝑘𝑤𝑘 (3.14)
𝐻𝑘𝑥̂𝑘|𝑘−1 về cơ bản là dự đoán của phép đo ở trạng thái k Hiệp phương sai của sai số ước lượng [9, tr.48]:
𝑃𝑘|𝑘 = 𝐸(𝑥̃𝑘|𝑘𝑥̃𝑘|𝑘𝑇 | 𝑍𝑘) = (1 − 𝑊𝑘𝐻𝑘)𝑃𝑘|𝑘−1(1 − 𝑊𝑘𝐻𝑘)𝑇+ 𝑊𝑘𝑅𝑘𝑊𝑘𝑇 (3.15)
𝑊𝑘là hệ số khuếch đại Kalman được chọn để tối thiểu hóa trung bình bình phương sai số ước lượng 𝐿𝑘 = 𝐸(𝑥̃𝑘|𝑘𝑇 𝑥̃𝑘|𝑘) là nhỏ nhất
𝑊𝑘 = 𝑃𝑘|𝑘−1𝐻𝑘𝑇[𝐻𝑘𝑃𝑘|𝑘−1𝐻𝑘𝑇 + 𝑅𝑘]−1 (3.16) Các phương trình (3.8), (3.9), (3.16), (3.13) và (3.15) là các phương trình cơ bản của bộ lọc Kalman rời rạc
Bộ lọc Kalman được ứng dụng thay thế bộ lọc thông thấp trong mô hình bộ lọc bù ở hình 3.8
Phương trình trạng thái và phương trình đo được viết tương đồng như sau:
[𝑑𝑁𝑘 𝑑𝐸𝑘] = [1 0 0 1] [ 𝑑𝑁𝑘−1 𝑑𝐸𝑘−1] + [1 1] 𝑣𝑘 (3.17) [𝑑𝑁𝑘(đ𝑜) 𝑑𝐸𝑘(đ𝑜)] = [1 0 0 1] [ 𝑑𝑁𝑘 𝑑𝐸𝑘] + 𝑤𝑘 (3.18) Các ma trận véc tơ trạng thái, ma trận hiệu phương sai của sai số ước lượng, ma trận hiệp phương sai của véc tơ nhiễu trạng thái và nhiễu đo được khởi tạo ban đầu:
𝑋0 = [0 0] ; 𝑃0 = [1 0 0 1] ; 𝑄 = [0.32 0.32]; 𝑅 = [1.52 1.52] _ 𝑥̂𝑘|𝑘−1 = 𝐹𝑘𝑥̂𝑘−1|𝑘−1+ 𝐵𝑘𝑢𝑘 𝑃𝑘|𝑘−1 = 𝐹𝑘𝑃𝑘−1|𝑘−1𝐹𝑘𝑇 + 𝐺𝑘𝑄𝑘𝐺𝑘𝑇 𝑊𝑘 = 𝑃𝑘|𝑘−1𝐻𝑘𝑇[𝐻𝑘𝑃𝑘|𝑘−1𝐻𝑘𝑇 + 𝑅𝑘]−1 𝑥̂𝑘|𝑘 = 𝑥̂𝑘|𝑘−1+ 𝑊𝑘(𝑧𝑘 − 𝐻𝑘𝑥̂𝑘|𝑘−1) 𝑃𝑘|𝑘 = (1 − 𝑊𝑘𝐻𝑘)𝑃𝑘|𝑘−1(1 − 𝑊𝑘𝐻𝑘)𝑇 + 𝑊𝑘𝑅𝑘𝑊𝑘𝑇
Hình 3.10 – Sơ đồ thuật toán của bộ lọc Kalman rời rạc
Vị trí tính bằng pp thủ công Vị trí tính bởi hệ thống USBL + Bộ lọc Kalman + Hình 3.11 – Sơ đồ áp dụng bộ lọc Kalman Bước cập nhật Bước dự báo + + + Vị trí ước lượng