Giả sử trạng thái của hệ thống biến đổi tại thời điểm k liên hệ với trạng thái k-1 bởi phương trình trạng thái [6]:
xk = f ( xk −1,uk −1) + wk −1 (3 69) Đo lường của hệ thống có quan hệ với trạng thái của hệ thống qua phương trình đo lường: yk = g ( xk ) + vk (3 70) xk +1 = Ak xk + Bkuk + wk = C x +v xk = Axk −1 + Bkuk −1 ( Lk k k k k kT= P H H P H + R k k k
trong đó: wk −1 là nhiễu của hệ thống, vk là nhiễu đo lường
Nhiệm vụ của bộ lọc Kalman là dựa vào quan sát và mô hình hệ thống để ước lượng tối ưu trạng thái của hệ thống
-Nếu cả hai phương trình trên là tuyến tính, nhiễu hệ thống và nhiễu đo lường là nhiễu gaussian thì bộ lọc Kalman tuyến tính (mục 3 3 1) là bộ lọc tối ưu được sử dụng trong trường hợp này
- Nếu ít nhất một trong hai phương trình trên là phi tuyến và nhiễu tác động không phải là nhiễu gaussian thì cần phải tuyến tính hóa hai phương trình trên dựa vào xấp xỉ Taylor bậc 1 Bộ quan sát trạng thái lúc này gọi là bộ lọc Kalman mở rộng sẽ là [6]:
xk = xk + A( xk −1 − xk −1) + wk −1
zk = zk + H ( xk − xk ) + vk
(3 71) trong đó: A, H là các ma trận Jacobian
Quá trình tổng hợp bộ lọc được thực hiện như sau: Từ phương trình trạng thái, biết trước xác suất tiền nghiệm:
x−+1 = f ( xk ,uk )
Pk−+1 = Ak Pk AT +W k QkWk
(3 72) Phương trình đo lường xác suất tiền nghiệm được cập nhật bởi xác suất hậu nghiệm Sau khi đệ quy sẽ xác định được trạng thái thệ thống [6]:
− T −xk = xk + Lk ( zk − Hx− ) xk = xk + Lk ( zk − Hx− ) Pk = ( I − Lk Hk ) Pk− )−1 (3 73) (3 74) (3 75) Chương trình được viết trên phần mềm Matlab tính toán bộ lọc Kalman mở rộng được trình bày trong phần phụ lục
77 k k ( Lk k k k k kT= P H H P H R +Vk k kT V k