Thuật toán Kalman rời rạc

y Lh LL hu  Nếu Lh g( rf1 

3.3.2. Thuật toán Kalman rời rạc

Bộ lọc Kalman đề cập đến bài toán tổng quát đi ước lượng trạng thái của một quá trình được mô hình hóa một cách rời rạc theo thời gian bằng một phương trình ngẫu nhiên tuyến tính như sau

xk A x* k1B u* k1wk1 (3.33) và kết quả đo đạc : zk H x* kvk (3.34)

Trong đó w và v là 2 vector biến ngẫu nhiên đại diện cho nhiễu hệ thông và nhiễu đo đạc. 2 biến ngẫu nhiên này độc lập và được giả sử là tuân theo phân bố Gauss

với trung bình bằng 0 và ma trận hiệp biến (covariance) lần lượt là Q và R w ~N(0,Q)

v ~N(0,R)

Nếu vector trạng thái x có kích thước là n, thì ma trận A sẽ có kích thước là ( n x n).Ma trận B (n x l) là ma trận phụ thuộc vào điều khiển tối ưu u với u là vector có kích thước là l. Vector đo đạc z có kích thước là m nên ma trận H sẽ là (m x n). Chú ý rằng các ma trận Q, R, ,H có thể thay đổi theo thời gian (từng bước k), nhưng ở đây chúng được giả sử không đổi. Đến đây ta thấy bài toán lọc Kalman chính là đi tìm giá trị ược lượng và ước đoán của trạng thái x khi ta biết được sự biến thiên của nó và ta đo được một đai lượng z mà phụ thuộc tuyến tính vào x. Ví dụ trong bài toán chuyển động, ta không biết được qui luật thay đổi của vận tốc, nhưng ta lại có thể đo được sự thay đổi của vị trí. hi đó, cái ta cần tìm là vận tốc ước lượng. Nếu ta giả sử xk và xk lần lượt là giá trị tiên nghiệm và giá trị tiền nghiệm ước lượng của giá trị x tại thời điểm k. iá trị tiên nghiệm thu được chỉ dựa vào mô hình hệ thống (1), còn giá trị hậu nghiệm là giá trị thu được sau khi đã có kết quả đo đạc zk

(2). hi đó sai số của ước đoán tiên nghiệm và hậu nghiệm lần lượt là k k k k k k e x x e x x       (3.35)

Ma trận hiệp biến của 2 sai số trên được tính lần lượt theo công thức

( ) ( ) T k k k T k k k P E e e P E e e     (3.36)

*( * )

k k k k

x xK z H x (3.37)

Phương trình (3.37) có nghĩa là giá trị nghiệm sau đó của ước lượng x sẽ được tính bằng giá trị tiên nghiệm của nó và sau đó thêm bớt đi một tí dựa vào sai số giữa giá trị đo được và giá trị đo đạc ước đoán H x* k . K ở đây chính là độ lợi

(gain) của mạch lọc Kalman. Câu h i đặt ra là làm thế nào để chọn K tối ưu nhất. Tối ưu ở đây theo nghĩa là covariance của sai số của ước lượng nghiệm sau đó

*( * k)

k k

e K z H x là nh nhất. Bằng cách thay ek vào trong biểu thức tính Pk, rồi sau đó lấy đạo hàm của Pk theo K, ta sẽ tìm ra được giá trị mà tương ứng với nó

k

P là nh nhất.

Kk P H HP Hk T( k T R)1

k