自动控制 / 系统课时
何时需要扩展卡尔曼滤波 EKF
何时需要扩展卡尔曼滤波 EKF
匀速目标可以用线性模型预测位置与速度,但距离、方位角传感器给出的并不是位置的线性组合。若仍把读数当成平面坐标使用,更新结果就会偏离真实几何。本篇用位于原点的雷达说明何时改用 EKF、雅可比怎样计算,以及如何验证一次完整更新。
学习目标
- 根据状态转移与观测函数,判断普通 KF 是否仍适用。
- 写出距离、方位角观测函数及其局部雅可比,正确处理角度新息。
- 用 NumPy 复现一次 EKF 更新,并识别其数值与建模边界。
根据观测关系选择滤波器
设状态为 。如果传感器直接输出平面位置 ,观测矩阵的两行就是 和 ;配合线性匀速状态转移,普通 KF 足够。
若雷达以传感器为原点,只返回距离 与方位角 ,目标位于 米时,读数是 米、。距离涉及平方根,角度涉及反正切,不能再用固定矩阵 直接乘状态;在局部线性化有效时应使用 EKF。

也可以先把极坐标换成 、,再尝试普通 KF;但此时测量噪声会随距离、方位角变化,还可能在两坐标轴间产生相关性,不能原样沿用极坐标噪声矩阵。若状态转移函数本身非线性,EKF 还需要在线性化预测侧处理它。
写出观测函数与雅可比
以传感器原点 为基准,观测函数是:
EKF 不把整个非线性函数替换成固定矩阵。它先得到预测状态 ,再在该点计算观测雅可比;记 ,有:
例如在 米处, 的两行分别为 与 。这只是该点附近的局部近似;目标移动后要重新求 。当 时方位角无定义,雅可比中的分母也会失效。
观测噪声协方差 的单位必须匹配观测:若距离标准差为 米、方位标准差为 ,应先把后者变为约 弧度,再取 ,而不是把数字 直接平方。
按预测、投影、更新执行
- **预测状态:**用匀速模型 和过程噪声 得到 、;本例每步间隔为 秒。
- **预测读数:**计算 ,并在同一个 处计算雅可比 ,不要在实测点求导。
- **形成新息:**用 计算距离与角度差,再把角度差归一化到 ;例如预测 、实测 ,正确新息为 ,不是 。
- **融合与传播:**计算 和增益 ,修正状态,使用 Joseph 形式更新协方差以减轻浮点误差影响。

用 NumPy 验证一次更新
以下程序适用于 Python 3.10+ 与 NumPy 1.24+。初始位置为 米,速度为 米/秒;一秒后雷达读数为 米、。两轴独立的单步加速度噪声标准差取 米/秒²。
import numpy as np
def h(x):
px, py = x[:2]
return np.array([np.hypot(px, py), np.arctan2(py, px)])
def jacobian(x):
px, py = x[:2]
r2 = px**2 + py**2
if r2 < 1e-8:
raise ValueError("距离过近,无法稳定线性化")
r = np.sqrt(r2)
return np.array([[px/r, py/r, 0., 0.],
[-py/r2, px/r2, 0., 0.]])
dt = 1.0
F = np.array([[1, 0, dt, 0], [0, 1, 0, dt],
[0, 0, 1, 0], [0, 0, 0, 1]], dtype=float)
G = np.array([[0.5*dt**2, 0], [0, 0.5*dt**2],
[dt, 0], [0, dt]])
Q = 0.04 * G @ G.T # 两轴独立的每步加速度噪声
R = np.diag([0.3**2, np.deg2rad(2.0)**2])
x = np.array([3.0, 4.0, 0.4, -0.2])
P = np.diag([0.25, 0.25, 0.04, 0.04])
z = np.array([5.25, np.deg2rad(52.0)])
# 先用线性的匀速运动模型预测
xp = F @ x
Pp = F @ P @ F.T + Q
# 只在当前预测状态处计算观测雅可比
H = jacobian(xp)
y = z - h(xp)
y[1] = (y[1] + np.pi) % (2 * np.pi) - np.pi
S = H @ Pp @ H.T + R
# 解线性方程组,避免显式计算 S 的逆矩阵
K = np.linalg.solve(S, H @ Pp).T
x_post = xp + K @ y
A = np.eye(4) - K @ H
P_post = A @ Pp @ A.T + K @ R @ K.T
nis = y @ np.linalg.solve(S, y)
print(f"预测位置: ({xp[0]:.3f}, {xp[1]:.3f}) m")
print(f"预测观测: {h(xp)[0]:.3f} m, {np.degrees(h(xp)[1]):.3f}°")
print(f"距离新息: {y[0]:+.3f} m")
print(f"方位新息: {np.degrees(y[1]):+.3f}°")
print(f"后验位置: ({x_post[0]:.3f}, {x_post[1]:.3f}) m")
print(f"后验速度: ({x_post[2]:.3f}, {x_post[3]:.3f}) m/s")
print(f"NIS: {nis:.3f}")
print(f"后验协方差对称: {np.allclose(P_post, P_post.T)}")预期输出:
预测位置: (3.400, 3.800) m
预测观测: 5.099 m, 48.180°
距离新息: +0.151 m
方位新息: +3.820°
后验位置: (3.248, 4.092) m
后验速度: (0.370, -0.142) m/s
NIS: 0.407
后验协方差对称: True位置从预测的 米调整为 米,速度也因位置与速度的互协方差而改变。这里的 取自预测位置 ,并非初始位置 ;输出保留三位小数,未显示的小数仍参与计算。
识别 EKF 的适用边界
EKF 适合观测函数可微、先验误差较集中且在当前预测附近近似线性的场景。代码中的距离下限只防止除以接近零的数;实际工程应依据雷达最小有效量程拒绝或另行处理近原点观测,不能把 当作正常方位角。
本例新息有两维,NIS 为 。若噪声模型近似正确,可用自由度为 的卡方分布进行单次门控,常用的 上限约为 ;一次通过只能说明本次新息不大,不能证明整段跟踪一致。若长期超限,按顺序检查角度单位、时间戳、 取值、雅可比以及传感器坐标系。
若预测距离仅 米,但位置不确定性标准差达到 米,预测分布横跨大量角度,单点线性化往往不足。可考虑不需要显式雅可比的 UKF;如果分布呈明显多峰,则考虑粒子滤波。对于直接输出笛卡尔位置、模型也线性的传感器,继续使用普通 KF 反而更简单。