Extended Kalman Filter (EKF)

Bộ lọc Kalman mở rộng (EKF) là một thuật toán đệ quy ước lượng trạng thái của hệ thống có chuyển động hoặc phép đo phi tuyến, chẳng hạn hướng của drone hoặc vị trí mục tiêu được radar xác định qua khoảng cách và góc phương vị. Bộ lọc Kalman (KF) tiêu chuẩn chỉ chính xác với model tuyến tính có nhiễu Gaussian. EKF xử lý model phi tuyến bằng cách tuyến tính hóa model quanh ước lượng hiện tại ở mỗi bước, sử dụng ma trận Jacobian của đạo hàm bậc nhất, rồi áp dụng các phương trình dự đoán và cập nhật quen thuộc. Gerald Smith, Stanley Schmidt và Leonard McGee tại NASA Ames phát triển thuật toán này vào đầu thập niên 1960 để dẫn đường trên khoang cho phương tiện bay vòng quanh Mặt Trăng; công trình này đóng góp vào máy tính dẫn đường Apollo. EKF vẫn là tiêu chuẩn thực tế để ước lượng trạng thái phi tuyến trong các hệ thống dẫn đường, bộ thu GPS, drone và robot.

Cách EKF hoạt động#

Giống bộ lọc Kalman, EKF duy trì một ước lượng trạng thái x và một ma trận hiệp phương sai P mô tả độ bất định của ước lượng. Tuy nhiên, model chuyển động f và model đo lường h có thể là các hàm khả vi bất kỳ. Mỗi bước gồm hai giai đoạn:

1. Dự đoán: Ước lượng đi qua model chuyển động phi tuyến, x = f(x). Bộ lọc tính Jacobian F = ∂f/∂x tại ước lượng và làm tăng độ bất định bằng nhiễu quá trình Q: P = F P Fᵀ + Q.
2. Cập nhật: Một phép đo z được đưa vào. Bộ lọc tính Jacobian đo lường H = ∂h/∂x tại trạng thái dự đoán, tính phần dư y = z − h(x) và độ lợi K = P Hᵀ (H P Hᵀ + R)⁻¹, trong đó R là nhiễu đo lường, rồi hiệu chỉnh ước lượng bằng x = x + K y và P = (I − K H) P.

Trạng thái và phép đo dự đoán đi qua các hàm phi tuyến thực; chỉ ma trận hiệp phương sai và độ lợi mới sử dụng Jacobian đã tuyến tính hóa. Khi f và h là tuyến tính, Jacobian bằng các ma trận model và EKF trở thành bộ lọc Kalman chính xác. Phép xấp xỉ có hiệu quả khi các model gần tuyến tính trong phạm vi phân bố của độ bất định và kém chính xác hơn khi chúng cong mạnh trong phạm vi đó.

Radar đo khoảng cách và góc phương vị là trường hợp kinh điển: mục tiêu chuyển động trên đường thẳng theo tọa độ x và y là tuyến tính trong các tọa độ đó, nhưng khoảng cách √(x² + y²) và góc phương vị atan2(y, x) thì không. EKF này theo dõi mục tiêu như vậy và ước lượng vận tốc của mục tiêu, thông số mà radar không bao giờ đo trực tiếp:

import numpy as np

rng = np.random.default_rng(0)
F = np.eye(4) + np.eye(4, k=2)  # constant velocity, state = [x, y, vx, vy], 1 s per step
Q, R = 1e-4 * np.eye(4), np.diag([1.0**2, 0.02**2])  # process noise; radar noise of 1 m and 0.02 rad
s = np.array([20.0, 50.0, 1.0, -0.5])  # true state
x, P = np.array([20.0, 50.0, 0.0, 0.0]), np.diag([1.0, 1.0, 4.0, 4.0])  # estimate starts with unknown velocity

raw_err, ekf_err = [], []
for _ in range(60):
    s = F @ s
    z = np.array([np.hypot(s[0], s[1]), np.arctan2(s[1], s[0])]) + rng.normal(0, [1.0, 0.02])  # range, bearing
    x, P = F @ x, F @ P @ F.T + Q  # predict
    r = np.hypot(x[0], x[1])
    H = np.array([[x[0] / r, x[1] / r, 0, 0], [-x[1] / r**2, x[0] / r**2, 0, 0]])  # Jacobian of h(x)
    K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)
    x = x + K @ (z - np.array([r, np.arctan2(x[1], x[0])]))  # update with the nonlinear residual
    P = (np.eye(4) - K @ H) @ P
    raw_err.append(np.hypot(z[0] * np.cos(z[1]) - s[0], z[0] * np.sin(z[1]) - s[1]))
    ekf_err.append(np.hypot(x[0] - s[0], x[1] - s[1]))

print(f"Mean position error: raw radar {np.mean(raw_err):.2f} m, EKF {np.mean(ekf_err):.2f} m")
print(f"Estimated velocity: ({x[2]:.2f}, {x[3]:.2f}) m/s, true (1.00, -0.50)")

Chuyển từng khoảng cách và góc phương vị thô sang x và y cho sai số vị trí trung bình 1.43 m trong 60 bước, trong khi ước lượng EKF sai lệch 0.57 m; bộ lọc khôi phục vận tốc thành (1.01, −0.50) m/s.

Hạn chế và biến thể#

EKF không phải là bộ ước lượng tối ưu. Nếu ước lượng ban đầu sai lệch nhiều hoặc các model phi tuyến mạnh, sai số tuyến tính hóa có thể khiến bộ lọc quá tự tin rồi mất hội tụ; vì vậy, các kỹ sư thường tăng nhiễu quá trình để đảm bảo an toàn. Jacobian phải được tính thủ công hoặc bằng vi phân tự động, và đạo hàm sai là nguyên nhân phổ biến gây lỗi âm thầm. Phần dư của các góc như góc phương vị hoặc hướng cũng cần được đưa về phạm vi từ −π đến π. Một số biến thể giải quyết những vấn đề này: EKF lặp tuyến tính hóa lại model đo lường trong quá trình cập nhật; EKF trạng thái sai số ước lượng một hiệu chỉnh nhỏ quanh trạng thái danh định, đây là cách phổ biến để theo dõi hướng 3D bằng quaternion trong dẫn đường quán tính.

So sánh EKF với các bộ lọc khác#

Việc chọn bộ lọc phụ thuộc vào mức độ phi tuyến của các model và hình dạng của độ bất định:

• Bộ lọc Kalman: Tối ưu và ít tốn kém hơn khi cả hai model đều tuyến tính. Các tracker hộp giới hạn như ByteTrack đo trực tiếp tọa độ hộp, vì vậy chỉ cần bộ lọc tuyến tính.
• Bộ lọc Kalman không mùi (UKF): Đưa một tập nhỏ các điểm sigma xác định qua các hàm phi tuyến thay vì tuyến tính hóa chúng; không cần Jacobian và thường chính xác hơn EKF với các model cong mạnh.
• Bộ lọc hạt: Biểu diễn phân bố xác suất bằng nhiều mẫu ngẫu nhiên có trọng số, nhờ đó có thể theo dõi các phân bố đa đỉnh, chẳng hạn khi robot không biết mình đang ở hành lang nào trong hai hành lang; đổi lại, chi phí tính toán cao hơn nhiều.
• Model Markov ẩn (HMM): Áp dụng cùng quy trình đệ quy dự đoán và cập nhật cho một tập hữu hạn các trạng thái rời rạc.

Ứng dụng thực tế#

• Drone và máy bay: Bộ ước lượng EKF2 của hệ thống lái tự động PX4 chạy EKF 24 trạng thái, kết hợp dữ liệu IMU, GPS, từ kế, khí áp kế và các cảm biến khác để ước lượng hướng, vận tốc, vị trí, độ lệch cảm biến, từ trường Trái Đất và gió.
• Robot: Gói robot_localization của ROS cung cấp các node EKF và UKF để kết hợp odometry bánh xe, IMU và GPS; EKF-SLAM là phương pháp kinh điển cho định vị và lập bản đồ đồng thời. Odometry thị giác-quán tính dựa trên EKF, chẳng hạn bộ lọc Kalman ràng buộc đa trạng thái (MSCKF), theo dõi tư thế camera từ feature ảnh và số liệu IMU.
• Kết hợp cảm biến trên ô tô: Các hệ thống hỗ trợ lái và xe tự hành kết hợp khoảng cách và góc phương vị từ radar với detection từ camera và lidar để theo dõi vị trí, vận tốc của các xe xung quanh.
• Định vị bằng camera: Một camera đơn chỉ đo được hướng đến vật thể. Các pipeline đưa vật thể được phát hiện lên bản đồ sử dụng detection từ Ultralytics YOLO26 làm phép đo cho EKF: tâm mỗi box là vị trí pixel, tương ứng với phép chiếu phi tuyến vị trí 3D của vật thể qua model camera; bộ lọc kết hợp vị trí này với khoảng cách từ radar hoặc dữ liệu GPS và IMU của nền tảng camera.