自动控制 / 系统课时

何时需要扩展卡尔曼滤波 EKF

何时需要扩展卡尔曼滤波 EKF

匀速目标可以用线性模型预测位置与速度,但距离、方位角传感器给出的并不是位置的线性组合。若仍把读数当成平面坐标使用,更新结果就会偏离真实几何。本篇用位于原点的雷达说明何时改用 EKF、雅可比怎样计算,以及如何验证一次完整更新。

学习目标

  • 根据状态转移与观测函数,判断普通 KF 是否仍适用。
  • 写出距离、方位角观测函数及其局部雅可比,正确处理角度新息。
  • 用 NumPy 复现一次 EKF 更新,并识别其数值与建模边界。

根据观测关系选择滤波器

设状态为 x=[px,py,vx,vy]Tx=[p_x,p_y,v_x,v_y]^T。如果传感器直接输出平面位置 (px,py)(p_x,p_y),观测矩阵的两行就是 [1,0,0,0][1,0,0,0] 和 [0,1,0,0][0,1,0,0];配合线性匀速状态转移,普通 KF 足够。

若雷达以传感器为原点,只返回距离 rr 与方位角 θ\theta,目标位于 (3,4)(3,4) 米时,读数是 r=5r=5 米、θ≈53.13∘\theta\approx53.13^\circ。距离涉及平方根,角度涉及反正切,不能再用固定矩阵 HH 直接乘状态;在局部线性化有效时应使用 EKF。

直角坐标观测与雷达距离方位角观测的区别

也可以先把极坐标换成 x=rcos⁡θx=r\cos\theta、y=rsin⁡θy=r\sin\theta,再尝试普通 KF;但此时测量噪声会随距离、方位角变化,还可能在两坐标轴间产生相关性,不能原样沿用极坐标噪声矩阵。若状态转移函数本身非线性,EKF 还需要在线性化预测侧处理它。

写出观测函数与雅可比

以传感器原点 (0,0)(0,0) 为基准,观测函数是:

h(x)=[px2+py2atan2⁡(py,px)]h(x)=\begin{bmatrix}\sqrt{p_x^2+p_y^2}\\\operatorname{atan2}(p_y,p_x)\end{bmatrix}

EKF 不把整个非线性函数替换成固定矩阵。它先得到预测状态 x−x^-,再在该点计算观测雅可比;记 r=px2+py2r=\sqrt{p_x^2+p_y^2},有:

H=∂h∂x∣x−=[px/rpy/r00−py/r2px/r200]H=\left.\frac{\partial h}{\partial x}\right|_{x^-}=\begin{bmatrix}p_x/r&p_y/r&0&0\\-p_y/r^2&p_x/r^2&0&0\end{bmatrix}

例如在 (px,py)=(3,4)(p_x,p_y)=(3,4) 米处,HH 的两行分别为 [0.6,0.8,0,0][0.6,0.8,0,0] 与 [−0.16,0.12,0,0][-0.16,0.12,0,0]。这只是该点附近的局部近似;目标移动后要重新求 HH。当 r=0r=0 时方位角无定义,雅可比中的分母也会失效。

观测噪声协方差 RR 的单位必须匹配观测:若距离标准差为 0.30.3 米、方位标准差为 2∘2^\circ,应先把后者变为约 0.03490.0349 弧度,再取 R=diag⁡(0.32,0.03492)R=\operatorname{diag}(0.3^2,0.0349^2),而不是把数字 22 直接平方。

按预测、投影、更新执行

  1. **预测状态:**用匀速模型 FF 和过程噪声 QQ 得到 x−=Fxx^-=Fx、P−=FPFT+QP^-=FPF^T+Q;本例每步间隔为 11 秒。
  2. **预测读数:**计算 h(x−)h(x^-),并在同一个 x−x^- 处计算雅可比 HH,不要在实测点求导。
  3. **形成新息:**用 y=z−h(x−)y=z-h(x^-) 计算距离与角度差,再把角度差归一化到 [−π,π)[-\pi,\pi);例如预测 179∘179^\circ、实测 −179∘-179^\circ,正确新息为 +2∘+2^\circ,不是 −358∘-358^\circ。
  4. **融合与传播:**计算 SS 和增益 KK,修正状态,使用 Joseph 形式更新协方差以减轻浮点误差影响。
S=HP−HT+R,K=P−HTS−1S=HP^-H^T+R,\qquad K=P^-H^TS^{-1}
x+=x−+Ky,P+=(I−KH)P−(I−KH)T+KRKTx^+=x^-+Ky,\qquad P^+=(I-KH)P^-(I-KH)^T+KRK^T

EKF在预测状态处线性化并融合极坐标测量的数据流

用 NumPy 验证一次更新

以下程序适用于 Python 3.10+ 与 NumPy 1.24+。初始位置为 (3,4)(3,4) 米,速度为 (0.4,−0.2)(0.4,-0.2) 米/秒;一秒后雷达读数为 5.255.25 米、52∘52^\circ。两轴独立的单步加速度噪声标准差取 0.20.2 米/秒²。

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

预期输出:

text
预测位置: (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

位置从预测的 (3.400,3.800)(3.400,3.800) 米调整为 (3.248,4.092)(3.248,4.092) 米,速度也因位置与速度的互协方差而改变。这里的 HH 取自预测位置 (3.4,3.8)(3.4,3.8),并非初始位置 (3,4)(3,4);输出保留三位小数,未显示的小数仍参与计算。

识别 EKF 的适用边界

EKF 适合观测函数可微、先验误差较集中且在当前预测附近近似线性的场景。代码中的距离下限只防止除以接近零的数;实际工程应依据雷达最小有效量程拒绝或另行处理近原点观测,不能把 r=0r=0 当作正常方位角。

本例新息有两维,NIS 为 0.4070.407。若噪声模型近似正确,可用自由度为 22 的卡方分布进行单次门控,常用的 95%95\% 上限约为 5.995.99;一次通过只能说明本次新息不大,不能证明整段跟踪一致。若长期超限,按顺序检查角度单位、时间戳、R/QR/Q 取值、雅可比以及传感器坐标系。

若预测距离仅 22 米,但位置不确定性标准差达到 1010 米,预测分布横跨大量角度,单点线性化往往不足。可考虑不需要显式雅可比的 UKF;如果分布呈明显多峰,则考虑粒子滤波。对于直接输出笛卡尔位置、模型也线性的传感器,继续使用普通 KF 反而更简单。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

何时需要扩展卡尔曼滤波 EKF

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通