Lọc Kalman là một thuật toán xử lý dữ liệu đệ quy tối ưu kết hợp tất cả dữ liệu phép đo có thể và sử dụng một kết quả tiền nghiệm về hệ thống và phép đo để
51
tạo ra một ước lượng của những biến mong muốn với một sai số thống kê tối thiểu. Lọc Kalman xử lý tất cả các phép đo có thể không quan tâm tới độ chính xác của chúng, sử dụng bất kỳ thông tin có thể nào như điều kiện ban đầu của những biến trạng thái và sự mô tả thống kê của những nhiễu hệ thống và những sai số phép đo. Lọc Kalman là đệ quy bởi vì nó không yêu cầu tất cả dữ liệu trước đó để xử lý và tái xử lý khi một phép đo mới được thực hiện. Lọc Kalman là tối ưu vì nó được thiết kế để trở thành phương thức ước lượng nhất quán và không chênh lệch với một sai phân bình thường tối thiểu được thể hiện trong phương trình dưới đây[20]:
lim 𝑛→∞𝑃(|𝑥̂ − 𝑥| < 𝜀) = 1 (4.2.1) 𝐸(𝑥̂) = 𝑥 (4.2.2) 𝐸 {(𝑥̂ − 𝐸(𝑥̂))𝑇(𝑥̂ − 𝐸(𝑥̂))} = 𝑚𝑖𝑛 (4.2.3) Trong đó n là kích thước mẫu; x là vecto trạng thái;
𝑥̂ là ước lượng của vecto trạng thái;
𝜀 là một giá trị rất nhỏ; P( ) là xác suất thống kê; E( ) là kì vọng thống kê;
Trong lọc Kalman rời rạc, phương trình trạng thái và phương trình phép đo có thể được viết như sau:
𝑥𝑘 = 𝜙𝑘−1𝑥𝑘−1+ 𝑤𝑘−1 (4.2.4)
𝑧𝑘 = 𝐻𝑘𝑥𝑘 + 𝜈𝑘 (4.2.5) Trong đó
𝑥𝑘, 𝑥𝑘−1 là vecto trạng thái tương ứng tại giai đoạn k và k-1;
𝜙𝑘−1 là ma trận chuyển đổi liên kết trạng thái tại bước thời gian trước k-1 và trạng thái tại bước thời gian hiện tại k;
𝑧𝑘 là vecto phép đo;
52
𝑤𝑘 và 𝜈𝑘 tương ứng là biến ngẫu nhiên thể hiện quá trình xử lý và nhiễu phép đo. Chúng được cho là độc lập và tuân theo phân phối xác suất thông thường và được cho bởi phương trình sau:
𝑤𝑘−1~𝑁(0, 𝑄𝑘−1) (4.2.6)
𝜈𝑘~𝑁(0, 𝑅𝑘) (4.2.7)
Trong đó Q và R tương ứng là ma trận xử lý nhiễu phương sai và phép đo nhiễu phương sai.
Lọc Kalman rời rạc bao gồm hai bước chính: cập nhật thời gian và cập nhật phép đo. Phương trình cập nhật thời gian chịu trách nhiệm đạt được ước lượng tiền nghiệm của trạng thái hiện tại và sai số phương sai cho bước tiếp theo, trong khi phương trình cập nhật phép đo chịu trách nhiệm hợp nhất một phép đo mới với ước lượng tiền nghiệm để thu được một ước lượng hoàn thiện. Phương trình cập nhật thời gian cũng là phương trình dự đoán, và phương trình cập nhật phép đo cũng được quy như phương trình hiệu chỉnh. Phương trình đặc trưng cho cập nhật thời gian và phép đo được cho như sau:
Dự đoán: 𝑥̂𝑘̅ = 𝜙𝑘−1𝑥̂𝑘−1 (4.2.8) 𝑃𝑘̅ = 𝜙𝑘−1𝑃𝑘−1𝜙𝑇 𝑘−1+ 𝑄𝑘−1 (4.2.9) Hiệu chỉnh: 𝐾𝑘 = 𝑃𝑘̅𝐻𝑇 𝑘(𝐻𝑘𝑃𝑘̅𝐻𝑇 𝑘 + 𝑅𝑘)−1 (4.2.10) 𝑥̂𝑘 = 𝑥̂𝑘̅ + 𝐾𝑘(𝑧𝑘 − 𝐻𝑘𝑥̂𝑘̅) (4.2.11) 𝑃𝑘 = (1 − 𝐾𝑘𝐻𝑘)𝑃𝑘̅ (4.2.12) Trong đó
𝑥̂𝑘̅, 𝑥̂𝑘 tương ứng là vecto trạng thái dự đoán và cập nhật;
𝑃𝑘̅, 𝑃𝑘 tương ứng là ma trận phương sai trạng thái dự đoán và cập nhật;
53
Hình 4- 1: Mô tả hoạt động lọc Kalman