Lý thuyết Bayes
P(X=x Z=z1,z2, zn )
P(Z=zn X=x)
P(Z=z2 X=x)
P(Z=z1 X=x)
Sen sơ 2: Hiệu chỉnh/ Ước tính Sen sơ 1: Hiệu chỉnh/
Ước tính
Sen sơ n: Hiệu chỉnh/ Ước tính
Mạch logic chọn: - MAP - MMSE
2.1.2. Phương pháp sử dụng bộ lọc Kalman cho bài toán định vị robot
Bộ lọc Kalman (Kalman Filter) rời rạc sử dụng mơ hình tốn học cho lọc các tín hiệu bằng cách sử dụng các phép đo với một số lượng đáng kể các sai số thống kê và có hệ thống được phát triển bởi Kalman và Bucy vào năm 1960 [47]. Thông thường, một bộ lọc Kalman tổng hợp dữ liệu đo được trong một khoảng thời gian liên tiếp, cung cấp một ước tính khả năng xảy ra lớn nhất của một tham số. Nó cũng có thể liên kết với các đầu vào của đa cảm biến thành một véc tơ của các trạng thái bên trong có chứa các tham số quan tâm, miễn là chúng chỉ phụ thuộc tuyến tính giữa các đầu vào và các trạng thái của hệ thống [91]. Bộ lọc sử dụng thuật toán thời gian rời rạc để loại bỏ nhiễu từ các tín hiệu của cảm biến để tạo ra được dữ liệu tổng hợp, ví dụ ước tính các giá trị làm trơn của vị trí, vận tốc, và gia tốc tại một dãy các điểm trong quỹ đạo. Mơ hình bộ lọc Kalman chuẩn được suy ra từ 2 phương trình tuyến tính. Phương trình thứ nhất mơ tả động học của hệ thống:
xk = Axk −1 + Buk −1 + wk −1 (2.4)
Trong đó:
xk là véc tơ trạng thái của hệ thống tại thời điểm k .
A là ma trận trạng thái không suy biến.
Véc tơ uk −1 miêu tả đầu vào hệ thống tại thời điểm k - 1.
Mối quan hệ véc tơ đầu vào uk −1 và véc tơ trạng thái xk được xác định bằng ma trận B
, wk −1 là một biến ngẫu nhiên biểu diễn cho nhiễu hệ thống. Phương trình tuyến tính thứ 2 mô tả quan sát nhiễu của hệ thống, với phép
đo z ∈ℜm :
zk = Hxk + vk (2.5)
Trong mỗi phần tử của vec-tơ zk chứa một quan sát cảm biến tại thời điểm k . Ma trận H liên quan tới các phép đo trạng thái bên trong, vk
biểu diễn nhiễu đo. Các biến ngẫu nhiên wk
và
vk được giả định là nhiễu quá trình và nhiễu đo; độc lập
k k 0 0 0 k wk ∼ N (0, Qk ); v ∼ N (0, R ); E(w vT ) = 0 k k i j
Qk là các 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. Bài tốn bộ lọc Kalman chính là tìm giá trị ước lượng của trạng thái x khi biết được sự biến thiên của nó mà ta đo được một đại lượng z phụ thuộc tuyến tính vào x . Nói cách khác, dựa trên các số liệu đo z , có thể tìm được một hệ số Kalman K trong mỗi chu kỳ lấy mẫu tín hiệu đo thuộc một vịng lặp đệ quy gọi là bộ lọc Kalman, sao cho giá trị ước lượng trạng thái của hệ gần với giá trị thực nhất. Như vậy, quá trình thực hiện của bộ lọc Kalman là một vòng lặp đệ quy gồm 2 giai đoạn là pha dự đoán (với các phương trình cập nhật thời gian) và pha hiệu chỉnh (với
các phương trình cập nhật số liệu). Hệ số Kalman Kk sẽ được tính tốn, sau đó véc tơ trạng thái và ma trận lỗi tương quan Pk sẽ được cập nhật từ các đánh giá bước trước
xˆ− và
P− . Để khởi động bộ lọc Kalman, người ta phải cung cấp một ước tính
tiên nghiệm xˆ− và một ước tính hiệp phương sai lỗi của
nó P
− . P− có thể được khởi
tạo với một ước tính giá trị ban đầu khơng chính xác, khi đó ứng dụng tiếp theo của bộ lọc Kalman sẽ cho
P− tiếp cận gần với giá trị chính xác của nó.