EKF (Extended Kalman Filter): thuật toán đứng sau sensor fusion

Bài Sensor Fusion nhắc tới robot_localization chạy "một EKF" nhưng chưa giải thích cơ chế bên trong. EKF (Extended Kalman Filter) là thuật toán cụ thể trả lời câu hỏi "cân trọng số các nguồn dữ liệu như thế nào" — bằng cách theo dõi không chỉ ước lượng vị trí, mà cả độ không chắc chắn của chính ước lượng đó.

Khái niệm chính

Khác với chỉ lưu một con số vị trí (x, y, θ), Kalman Filter lưu thêm một ma trận covariance — biểu diễn robot "tự tin" tới mức nào về ước lượng hiện tại. Hình dung trực quan: thay vì một điểm chấm duy nhất trên bản đồ, ước lượng là một hình elip — elip nhỏ nghĩa là rất tự tin, elip lớn nghĩa là không chắc chắn.

Nguyên lý hoạt động

PREDICT (dự đoán):
    dùng mô hình chuyển động (odometry/IMU) để ước lượng vị trí mới
    → elip không chắc chắn PHÌNH TO thêm (dead reckoning luôn thêm sai số)

UPDATE (cập nhật):
    có phép đo mới (ví dụ so khớp LiDAR với bản đồ)
    → kết hợp phép đo mới với dự đoán, elip không chắc chắn CO LẠI

Hai bước lặp lại liên tục: mỗi khi có dữ liệu chuyển động mới (predict), độ không chắc chắn tăng lên; mỗi khi có phép đo tuyệt đối mới (update), độ không chắc chắn giảm xuống — chính vòng lặp phình-co này là cách Kalman Filter "tự động cân trọng số".

Tóm lại: Trọng số dành cho phép đo mới (gọi là Kalman Gain) không cố định — nó tính toán động dựa trên tỉ lệ giữa độ không chắc chắn của dự đoán và độ không chắc chắn của chính phép đo mới.

Kalman Filter gốc chỉ đúng toán học hoàn hảo với hệ thống tuyến tính. Nhưng chuyển động thực tế của robot có sin(θ), cos(θ) — phi tuyến. EKF giải quyết bằng cách tuyến tính hoá cục bộ quanh điểm ước lượng hiện tại ở mỗi bước, dùng ma trận Jacobian để xấp xỉ hệ phi tuyến bằng một hệ tuyến tính "đủ gần đúng":

Kalman Filter gốc:  chỉ đúng với hệ tuyến tính hoàn toàn
EKF:                 xấp xỉ tuyến tính cục bộ (Jacobian) mỗi bước — dùng được cho hệ phi tuyến
UKF (Unscented KF):  một cách xấp xỉ khác, không cần tính đạo hàm, thường chính xác hơn EKF

Kiến thức kỹ thuật

Công thức

Dạng đơn giản hoá 1 chiều của Kalman Gain — công thức chính xác đứng sau lời giải thích "trọng số động" ở phần Nguyên lý hoạt động:

K = P / (P + R)

P = độ không chắc chắn của dự đoán (predict)
R = độ nhiễu của phép đo mới (measurement)

VD: P = 0.5 (dự đoán khá không chắc chắn), R = 0.1 (phép đo khá đáng tin)
K = 0.5 / (0.5 + 0.1) ≈ 0.83

Ước_lượng_moi = Du_doan + K × (Phep_do − Du_doan)
→ K gần 1: gần như tin hoàn toàn vào phép đo mới
→ K gần 0: gần như giữ nguyên dự đoán, ít tin phép đo

Thông số

Vector trạng thái đầy đủ mà robot_localization theo dõi trong EKF khớp chính xác với 15 phần tử trong mảng _config đã thấy ở bài Sensor Fusion:

[x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az]
→ 15 chiều trạng thái, mỗi chiều có độ không chắc chắn riêng trong ma trận covariance

Ứng dụng thực tế

Ngoài fusion odometry/IMU, chính thuật toán predict-update này còn là nền tảng của EKF-SLAM — một cách tiếp cận SLAM khác với phương pháp phổ biến hơn (particle-filter-based như gmapping): thay vì chỉ theo dõi vị trí robot, vector trạng thái EKF-SLAM còn chứa vị trí của từng landmark (điểm mốc) đã phát hiện được, và phát triển dần khi robot khám phá thêm landmark mới — mỗi lần robot nhìn thấy lại 1 landmark cũ, bước update không chỉ sửa lại vị trí robot mà còn sửa lại vị trí ước lượng của landmark đó, cải thiện dần cả bản đồ lẫn vị trí robot cùng lúc trong cùng 1 bộ lọc.

Ví dụ thực tế

def kalman_1d(prediction, P, measurement, R):
    K = P / (P + R)
    estimate = prediction + K * (measurement - prediction)
    P_new = (1 - K) * P
    return estimate, P_new

# Predict: du doan vi tri = 10.0, do khong chac chan P = 0.5
prediction, P = 10.0, 0.5
# Update: do luong duoc 10.3, nhieu do R = 0.1 (kha dang tin)
measurement, R = 10.3, 0.1

estimate, P_new = kalman_1d(prediction, P, measurement, R)
print(f"Du doan: {prediction} (P={P})")
print(f"Do luong: {measurement} (R={R})")
print(f"Uoc luong sau update: {estimate:.2f} (P moi={P_new:.3f})")

Output:
Du doan: 10.0 (P=0.5)
Do luong: 10.3 (R=0.1)
Uoc luong sau update: 10.25 (P moi=0.083)

Ước lượng cuối (10.25) nghiêng về phía phép đo (10.3) nhiều hơn dự đoán (10.0) vì phép đo đáng tin hơn (R nhỏ hơn P) — và độ không chắc chắn giảm từ 0.5 xuống 0.083 sau update, đúng hành vi "elip co lại" đã mô tả ở phần Khái niệm chính.

Lỗi thường gặp

• Khởi tạo covariance ban đầu quá nhỏ (quá tự tin) — nếu ước lượng khởi tạo sai mà covariance ban đầu quá nhỏ, Kalman Gain sẽ luôn thiên về tin "dự đoán" (vốn đã sai từ đầu) hơn phép đo mới, khiến bộ lọc hội tụ rất chậm về giá trị đúng hoặc không bao giờ hội tụ.
• EKF phân kỳ (diverge) khi chuyển động quá phi tuyến — xấp xỉ tuyến tính hoá bằng Jacobian chỉ chính xác trong lân cận nhỏ quanh điểm hiện tại — robot xoay quá nhanh hoặc chuyển động đột ngột mạnh có thể khiến xấp xỉ này sai lệch nghiêm trọng, gây ước lượng phân kỳ khỏi giá trị thật — đây là lý do UKF đôi khi được ưu tiên hơn cho robot chuyển động phi tuyến mạnh.
• Không tinh chỉnh ma trận nhiễu quá trình/phép đo theo dữ liệu thực tế — dùng giá trị mặc định tuỳ tiện cho P, R mà không đo đạc/hiệu chỉnh theo đặc tính cảm biến thực tế khiến Kalman Gain tính ra không phản ánh đúng độ tin cậy thật, phá vỡ toàn bộ lợi ích của sensor fusion.
• Kỳ vọng EKF xử lý được đa giả thuyết (multi-modal) — EKF giả định độ không chắc chắn có dạng phân phối Gauss đơn (1 "đỉnh" duy nhất) — không xử lý được tình huống "robot có thể đang ở 1 trong 2 vị trí rất khác nhau" (VD: vừa khởi động, chưa biết đang ở phòng nào) — đây chính là lý do AMCL (bài tiếp theo) dùng particle filter thay vì Kalman Filter cho bài toán định vị toàn cục.

Kết luận

EKF hiện thực hoá "tự động cân trọng số" của sensor fusion bằng vòng lặp predict-update rõ ràng, dùng Kalman Gain tính động dựa trên độ không chắc chắn tương đối giữa dự đoán và phép đo mới, mở rộng cho hệ phi tuyến qua tuyến tính hoá Jacobian. Đây là nền tảng toán học trực tiếp của robot_localization. Bài tiếp theo trong chuyên mục, cũng là bài cuối cùng của toàn bộ chuyên mục Robotics Fundamentals, nói về AMCL — kỹ thuật định vị toàn cục dùng particle filter, giải quyết đúng hạn chế "đơn giả thuyết" của EKF vừa nêu.