Phát triển dựa trên bộ lọc đơn giản, bây giờ chúng ta sẽ điều chỉnh bộ lọc Kalman trong môi trường đa chiều với nhiều feature. Chúng ta cần chú ý đến sự thay đổi giữa quan sát và di chuyển. Quan sát liên quan đến định vị feature được sử dụng để cập nhật và tăng sự tự tin và di chuyển sẽ cung cấp các điểm thuận lợi mới cho các robot trong khi giảm niềm tin vào trạng thái thế giới.
2. Những kí hiệu sử dụng trong môi trường đa chiều
Chúng ta cần một số cách thể hiện ước lượng vị trí feature. Trong môi trường một chiều, ta sử dụng biến z để quan sát. Chúng ta có thể đơn giản bằng cách thay đổi ý nghĩa của z để chỉ ước lượng thực tếvị trí feature và chỉ cần bỏ ký hiệu cho các quan sát. Ngoài ra, chúng ta cần quen với các ký hiệu để phân biệt giữa các feature ở một thời gian cụ thể i. Khi đó, zijthể hiện ước lượng vị trí feature thứ j vào thời điểm ở
Kalman Filter cho 1 chiều và 1 feature: Cập nhật trạng thái cho đầu vào sensor:
xi = xi-1 + Ki * [xs - xi-1] (2.4) Cập nhật không chắc chắn cho đầu vào sensor:
vari= vari-1- Ki* vari-1 (2.6)
Cập nhật trạng thái cho điều khiển đầu vào:
xi = xi-1 + ui * T (2.7) Cập nhật không chắc chắn cho điều khiển đầu vào:
vari= vari-1+ varu* T (2.8)
Trong đó giá trị trọng số của Kalman là:
bước i, zilà véc tơ của tất cả các quan sát vào thời điểm ở bước i: zi = [zi0, zi1... zin]. Lưu ý rằng zi0 là một véc tơ xác định vị trí cho feature 0.
Trong môi trường một chiều, ta chỉ cần một phương sai đơn biến var, nhưng điều này không còn đủ để mô hình hóa trong hệ thống. Sự không chắc chắn được lưu trong một ma trận phương sai P cần liên tục cập nhật.
Vector chuyển động ui là véc tơ chuyển động dự báo cho điều khiển đầu vào tại thời điểm bước i, nhưng điều này phải bao gồm cả vận tốc và hướng đa chiều.
Trong ví dụ một chiều, Ki là một trọng số dựa trên sự chênh lệch. Chức năng của nó sẽ được bảo tồn. Tương tự như vậy, một giá trị duy nhất thể hiện sự không chắc chắn vari sẽ không còn được đầy đủ, chúng ta sẽ cần ma trận Pi thể hiện sự không chắc chắn. Ki và Pi sẽ được xem xét trong thời gian ngắn hơn.
3. Phương trình chuyển động
Trong dạng tổng quát của các bộ lọc Kalman được sử dụng trong môi trường SLAM thực tế, các bản cập nhật cần thiết bởi việc điều khiển đầu vào được xử lý bởi “phương trình dự đoán”.Khi chúng ta di chuyển và cần phải “dự đoán” tháithay đổi như thế nào. Các dữ liệu cảm biến được xử lý bởi “phương trình cập nhật”, cập nhật vector trạng thái và chắc chắn dựa trên các dữ liệu. Ta xem xét phương trình dự đoán Phương trình dự đoán có đầu vào thiết bị chuyển động như sau:
xi = Fi * xi-1 + Gi * ui (2.9)
Trong đó:
Fi: là ma trận chuyển dịch trạng thái
Gi: là ma trận có thể dịch đầu vào vào sự thay đổi dự đoán được trong trạng thái. Phương trình 2.9phức tạp hơn của phương trình 2.7.Fimô tảtrạng thái sẽ thay đổi do các nhân tố không liên quan đến điều khiển đầu vào. Một quy ước rất có ích trong SLAM là cho rằng feature này là tĩnh.
Ngoại trừ hàng đầu tiên tương ứng với những thay đổi do robot gây ra, Fi như ma trận đường chéo với mỗi phần tử có chứa ma trận xác định (nếu Fi thay đổi vị trí của các featuremà chúng ta biết là tĩnh). Nếu robot của chúng ta có thể có một vận tốc khác 0 tại bước i, sau đó trạng thái có thể thay đổi ngay cả khi không có đầu vào của thiết bị truyền động, cột đầu tiên của Fi có thể giải thích cho việc này. Để đơn giản hơn một chút, giả sử rằng các robot dừng sau mỗi bước thời gian. Trong trường hợp này, các đầu vào thiết bị truyền động xác định đầy đủ các vị trí có nhiều feature mới của robot. Điều này có nghĩa là Fi là ma trận đơn vịhay là một ma trận đường chéo có đường chéo các phần tử là ma trận định danh và có thể được bỏ qua hoàn toàn. Như vậy, mục đích của chúng ta, đơn giản hóa công thức(2.9):
xi = xi-1 + Gi * ui (2.10)
4. Phương trình Sensor
Đặc điểm kỹ thuật của phương trình "dự đoán" trong các phần trước chỉ còn lại bộ cảm biến, hay "cập nhật", phương trình được sử dụng để kết hợp ước lượng feature dựa trên việc đọc cảm biến. Những phương trình này tương tự như phương trình 2.4 và 2.6 về hình thức, nhưng nó có giá trị để kiểm tra lần lượt để hiểu chính xác tình hình đã thay đổi như thế nào so với môi trường một chiều. Như thường lệ, chúng ta cần cập nhật trạng thái dự đoán và độ tin cậytrong trạng thái hiện tại bằng cách sử dụng dữ liệu mới. Chúng ta bắt đầu với phương trình cập nhật trạng thái:
xi = xi-1 + Ki * vs (2.13)
Trong đó vs là một vector điều chỉnh được đề xuất bởi việc đọc cảm biến mới được định nghĩa như sau:
vs = xs – Hi * xi – 1 (2.14)
Mục đích của Hi là chuyển đổi ước lượng trạng thái trước đó thành một thể hiện được sử dụng bởi các cảm biến. Nói cách khác, nếu các cảm biến đã nhận được trạng thái thế giới đúng như dự đoán của xi-1, chúngthể hiện thông tin dưới dạng Hi * xi - 1. Mục đích của Hi tương tự như của Gi. Thay cho việc sử dụng [phương trình 2.14] ta có:
xi = xi-1 + Ki * [xs – Hi * xi – 1] (2.15) Phương trình này rất giống với ví dụ môi trường một chiều. Tuy nhiên, Ki được xây dựng trong môi trường đa chiều và chứa nhiều feature. Ki xây dựng trên độ tin cậytương đối của cảm biến so với ước lượng trước như trong phần 2.4 Phát triển một bộ lọc đơn giản,nhưng Ki bây giờ đề cập đến một ma trận và được xác định như sau:
Ki = Pi-1 * HiT * Si-1 (2.16)
Si là hiệp phương sai được xác định như sau:
Si = Hi * Pi-1 * HiT + Rs (2.17) Trong đó RS là ma trận phương sai cho nhiễu cảm biến. Ri và Si là ma trận, với kích thước cộng với một số feature. Trong phần 2.4 Phát triển một bộ lọc đơn giản, nhiễu cảm biến có thể được biểu diễn chỉ với một biến duy nhất, VAR. Để hiểu tại sao Si và Ri là ma trận, chúng ta sử dụng khi định nghĩa Pi trong phần trước. Kết hợp [phương trình 2.16] và [phương trình 2.17] chúng ta thấy ma trận Kalman xây dựng theo cách này:
Ki = Pi-1 * HiT * [Hi * Pi-1 * HiT + Rs]-1 (2.18) Sau khi chúng ta đã cập nhật trạng thái, chúng ta sử dụng [phương trình 2.13] để cập nhật Pi như sau: