Extended Kalman Filter (EKF)

扩展卡尔曼滤波器（EKF）在每个步骤中使用雅可比矩阵将非线性系统模型线性化，以估计系统状态。附数学推导和代码。

扩展卡尔曼滤波器（EKF）是一种递归算法，用于估计运动或测量非线性的系统状态，例如无人机的姿态，或雷达根据距离和方位角测得的目标位置。标准卡尔曼滤波器（KF）仅适用于具有高斯噪声的线性模型。EKF 通过在每个步骤中使用一阶导数的雅可比矩阵围绕当前估计值进行线性化，从而处理非线性模型，然后应用熟悉的预测和更新方程。20 世纪 60 年代初，NASA 艾姆斯研究中心的 Gerald Smith、Stanley Schmidt 和 Leonard McGee 为绕月飞行器的机载导航开发了这种方法；这项工作为阿波罗导航计算机提供了支持。如今，它仍是导航系统、GPS 接收器、无人机和机器人中非线性状态估计的事实标准。

EKF 的工作原理#

与卡尔曼滤波器一样，EKF 会维护状态估计 x 和描述其不确定性的协方差 P，但运动模型 f 和测量模型 h 可以是任意可微函数。每个步骤包含两个阶段：

预测：估计值经过非线性运动模型 x = f(x)。滤波器在估计值处计算雅可比矩阵 F = ∂f/∂x，并使用过程噪声 Q 增大不确定性：P = F P Fᵀ + Q。
更新：收到测量值 z。滤波器在预测状态处计算测量雅可比矩阵 H = ∂h/∂x，再计算残差 y = z − h(x) 和增益 K = P Hᵀ (H P Hᵀ + R)⁻¹，其中 R 是测量噪声，然后使用 x = x + K y 和 P = (I − K H) P 修正估计值。

状态和预测测量值会经过真实的非线性函数；只有协方差和增益使用线性化的雅可比矩阵。当 f 和 h 是线性函数时，雅可比矩阵就等于模型矩阵，EKF 会精确退化为卡尔曼滤波器。当模型在不确定性范围内接近线性时，这种近似效果良好；若模型在该范围内急剧弯曲，近似效果就会变差。

测量距离和方位角的雷达是典型案例：沿 x 和 y 方向直线运动的目标在这些坐标下呈线性，但距离 √(x² + y²) 和方位角 atan2(y, x) 并非线性。这个 EKF 会跟踪这样的目标，并估计其速度，而雷达本身并不会测量速度：

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)")

将每个原始距离和方位角转换为 x 和 y 后，60 个步骤的平均位置误差为 1.43 米；EKF 估计的误差为 0.57 米，并且滤波器得出的速度为 (1.01, −0.50) 米/秒。

局限性与变体#

EKF 并非最优估计器。如果初始估计偏差很大，或模型高度非线性，线性化误差可能让滤波器过度自信，随后导致发散，因此工程师常会为安全起见增大过程噪声。雅可比矩阵必须手动推导或通过自动微分获得，而导数错误是导致隐蔽故障的常见原因。方位角或航向等角度的残差也需要限制在 −π 到 π 的范围内。多种变体可以解决这些问题：迭代 EKF 会在更新过程中重新线性化测量模型；误差状态 EKF 则会围绕名义状态估计一个较小的修正量，这是在惯性导航中使用四元数跟踪 3D 姿态的常见方法。

EKF 与其他滤波器的比较#

选择哪种滤波器，取决于模型的非线性程度以及不确定性的形态：

卡尔曼滤波器：当两个模型都是线性模型时，卡尔曼滤波器最优且成本更低。像 ByteTrack这样的边界框跟踪器会直接测量框坐标，因此线性滤波器足以满足需求。
无迹卡尔曼滤波器（UKF）：将一小组确定性的 sigma 点通过非线性函数，而不是对函数进行线性化；它不需要雅可比矩阵，并且在模型高度弯曲时通常比 EKF 更准确。
粒子滤波器：用许多加权随机样本表示信念状态，因此能跟踪多峰分布，例如机器人无法确定自己位于两条走廊中的哪一条；但计算成本要高得多。
隐马尔可夫模型（HMM）：对有限个离散状态应用相同的预测与更新递归过程。

实际应用#

无人机与飞机：PX4 自动驾驶仪的 EKF2 估计器运行一个 24 状态 EKF，将 IMU、GPS、磁力计、气压计和其他传感器的数据融合起来，估计姿态、速度、位置、传感器偏差、地球磁场和风。
机器人技术：ROS 的 robot_localization软件包提供 EKF 和 UKF 节点，用于融合轮式里程计、IMU 和 GPS；EKF-SLAM 是实现同时定位与建图的经典方法。基于 EKF 的视觉惯性里程计（例如多状态约束卡尔曼滤波器（MSCKF））会根据图像特征和 IMU 读数跟踪相机姿态。
汽车传感器融合：驾驶辅助和自动驾驶汽车系统会将雷达的距离和方位角与相机和激光雷达检测结果融合，以跟踪周围车辆的位置和速度。
基于相机的定位：单个相机只能测量物体的方向。将检测到的物体放置在地图上的处理流程会将Ultralytics YOLO26的检测结果作为 EKF 测量值：每个框的中心都是一个像素位置，它是物体 3D 位置经过相机模型非线性投影后的结果；滤波器会将其与雷达测得的距离，或相机平台的 GPS 和 IMU 数据融合。