Sơ đồ thuật tốn bộ lọc Kalman mở rộng

Bước dự đốn với các phương trình cập nhật:

xˆ  f (xˆ ,u ,0)

k k 1 k 1 (2.13)

P  AP AT  W Q WT

k k 1 k k 1 k (2.14) Bước hiệu chỉnh với các phương trình cập nhật:

K  PHT (H PHT  V R VT )1 k k k k k k k k k (2.15) xˆ  xˆ  K (z  h(xˆ,0)) k k k k k (2.16) P  (I  K H )P k k k (2.17) Trong đó:

x nk

xk  n

là ước lượng trạng thái tiên nghiệm ở bước k nhận được từ tiên nghiệm quá trình ở bước k .

là ước lượng trạng thái hậu nghiệm ở bước k - 1 nhận được sau phép đo zk

P là ma trận hiệp biến của sai số ước lượng trạng thái tiên nghiệm.

Pk là ma trận hiệp biến của sai số ước lượng trạng thái hậu nghiệm.

Qk 1 là ma trận hiệp phương sai nhiễu đầu vào.

Rk là ma trận hiệp phương sai của nhiễu đo.

Kk là hệ số lọc Kalman.

I là ma trận đơn vị.

Các biến ngẫu nhiên wk như (2.13) và (2.14).

và vk được giả định là nhiễu quá trình và nhiễu đo

A là ma trận Jacobian của các đạo hàm riêng của f theo x : Aij  fi x j ( x ˆ k 1 ,uk 1 , 0)

W là ma trận Jacobian của các đạo hàm riêng của f theo w :

Wij fi w j ( x ˆ k 1 ,uk 1, 0)

H là ma trận Jacobian của các đạo hàm riêng của h theo x : Hij  hi

(xˆ

xj

, 0)

V là ma trận Jacobian của các đạo hàm riêng của h theo v : Vij  hi (xˆ vj , 0) k k k

Qua những phân tích ở trên chúng ta thấy rằng khi sử dụng bộ lọc Kalman rời rạc cho phép đạt được nhiều kết quả mong muốn [90]. Tuy nhiên, mơ hình tuyến tính của hệ thống khơng phải lúc nào cũng khả thi. Vì vậy EKF đã được dẫn xuất từ các phương trình ngẫu nhiên phi tuyến cho mơ hình hệ thống [42]. Việc ứng

dụng EKF cho robot di động rất đa dạng, ví dụ như định vị cho robot di động có các bánh xe được điều khiển vi sai [38], hay ứng dụng EKF cho tổng hợp đa cảm biến để nâng cao khả năng ước lượng trạng thái của robot di động [44], [92]. Hơn nữa, trong một môi trường đa cảm biến với các ma trận đo lường giống hệt nhau, các phép đo có thể được xử lý một cách riêng biệt để có được độ phức tạp tính tốn thấp hơn. Trong trường hợp các ma trận đo không giống nhau là thích hợp sử dụng hợp nhất véc tơ đầu vào chứa các thông tin của tất cả các cảm biến. Thêm một lý do quan trọng khi tổng hợp đa cảm biến có nhiều loại khác nhau, do thời gian xử lý, thời gian cập nhật trên mỗi cảm biến khác nhau, nhưng vẫn có thể mơ hình hóa được với các dạng thơng tin của EKF và do đó có thể tổng hợp được nhiều cảm biến khác nhau, tăng số chiều dữ liệu [40].

SLAM được định nghĩa là một q trình mà ở đó robot có thể xây dựng bản đồ của môi trường, đồng thời sử dụng bản đồ đó để ước tính vị trí của robot trong mơi trường đó. Thuật tốn SLAM là cơng trình được nghiên cứu bởi các tác giả Smith, Self và Cheeseman [86], sau đó nó được phát triển bởi Hugh Durrant-Whyte và cộng sự [25], [53]. Thuật toán SLAM đề xuất vấn đề xây dựng bản đồ của môi trường không xác định cho robot di động đồng thời điều hướng môi trường bằng bản đồ [95]. Như trên Hình 2.4, thuật tốn SLAM được định nghĩa là việc ước tính vị trí vật cản mơi trường và vị trí của robot trong mơi trường đó.

Ở dạng xác suất, SLAM được biểu diễn dưới dạng phân bố xác suất sau:

P(xk ,m | z0:k ,u0:k ) (2.18)

Với u0:k = u0 , u1,..., uk  = u0:k-1, uk  : tập chứa các giá trị đầu vào điều khiển

từ thời điểm ban đầu đến thời điểm k ,

m = m1, m2,..., mn : tập các điểm mốc thể hiện vị trí vật cản. z0:k  z0, z1,..., zk  z0:k1, zk  : tập các điểm mốc quan sát được từ thời điểm ban đầu đến thời điểm k [68]. Phân bố xác suất này mô tả phân bố hậu

nghiệm liên kết giữa vị trí điểm mốc trong bản đồ m với trạng thái của robot xk tại thời điểm k khi biết giá trị quan sát được