Bộ lọc mở rộng Kalman (EKF) là một thuật toán được dùng rộng rãi trong định vị và hướng của robot bằng cách kết hợp dữ liệu cảm biến với mô hình toán học của robot. Hiểu được các nền tảng toán học của EKF là thiết yếu để thực hiện hiệu quả và chỉnh sửa.

Hình và dự đoán của nhà nước

Tâm của EKF bao gồm đại diện trạng thái của robot như là một véc tơ, thường bao gồm vị trí và hướng. Bước dự đoán sử dụng một mô hình chuyển động phi tuyến tính để dự đoán trạng thái hiện tại theo thời gian. Điều này bao gồm tính ma trận Jacobian của mô hình chuyển động để tuyến tính các phương trình không tuyến tính xung quanh ước tính hiện tại.

Đo và xếp hàng

Các phép đo cảm biến được cộng lại qua bước cập nhật. Vì các phép đo thường không tuyến tính của trạng thái, nên EKF tuyến tính hóa các chức năng này bằng cách sử dụng Jacobians. Quá trình này điều chỉnh trạng thái đã dự đoán dựa trên sự khác biệt giữa việc đọc và các phần nhạy thực sự.

Phương trình toán học

Phương trình tiên đoán là:

Dự đoán s

x tik + Grax = f(xxx , teppi82)

Dự đoán về sự bay hơi:)

Pk + Qk = Fk Pk Fkt

Fk là chuyển động không tuyến tính của nó Jacobian, P là các cùng biến đổi ma trận, và Q là quá trình nhiễu covariance.

Phương trình cập nhật là:

Thành công vô ích:)

Kk = Pk-Pk- x- ti- nạ (Hk Pk-Pk- Hkt + Rk) X1

Cập nhật

x tek = x vội + Kk (zk - h (xxxxkkGG))

Cập nhật cho máy bay:

Pk = (I - Kk Hk) Pk-Se- x

Ở đây, h là mô hình không tuyến tính đo lường, Hk là Jacobian của nó, Rk là đo lường nhiễu covariance, và zk là thực sự đo lường cảm biến.