自动控制 / 系统课时

过程噪声 Q 过小为何跟不上转弯

过程噪声 Q 过小为何跟不上转弯

小车直行时估计很平滑,一转弯却沿旧方向拖行,可能是匀速模型的过程噪声设得过小。通过同一组位置测量和三档 Q,你将复现速度修正不足、位置误差扩大,以及增大 Q 后响应改善但抖动增加的现象。

学习目标

  • 解释速率不变的转弯为何仍会破坏匀速模型假设。
  • 按状态顺序和 Δt=0.1 s\Delta t=0.1\,\mathrm{s} 构造过程噪声协方差。
  • 比较三档 Q 的直行、转弯误差,区分参数过小与运动模型不适用。

转弯怎样打破匀速假设

状态按 x=[px,py,vx,vy]Tx=[p_x,p_y,v_x,v_y]^T 排列,前两项是位置,后两项是速度。匀速模型让位置按当前速度外推,同时保持两个速度分量不变;因此,速率同为 2 m/s2\,\mathrm{m/s},由向东运动改成向北运动,依然是模型没有预测到的变化。

本例在 6 s 开始左转,经过 6 s 转过 90°,速度向量从 [2,0]T[2,0]^T 变成 [0,2]T[0,2]^T。滤波器只收到位置测量,不知道转弯开始时间,也没有转向控制输入;转向后的新位置必须通过测量更新逐步改变速度估计。

设 Pk−1P_{k-1} 为上拍后验协方差,HH 提取两个位置分量,RR 为测量噪声协方差。预测均值仍为 x^k−=Fx^k−1\hat x_k^-=F\hat x_{k-1},Q 通过以下关系影响修正力度:

Pk−=FPk−1FT+Q,Kk=Pk−HT(HPk−HT+R)−1,x^k=x^k−+Kk(zk−Hx^k−).\begin{aligned} P_k^- &= FP_{k-1}F^T+Q,\\ K_k &= P_k^-H^T(HP_k^-H^T+R)^{-1},\\ \hat x_k &= \hat x_k^-+K_k(z_k-H\hat x_k^-). \end{aligned}

Q 不直接向预测均值添加随机扰动。虽然传感器只读位置,P 中的位置与速度互协方差仍能让位置新息修正速度。直行阶段反复更新后,若 Q 太小,滤波器会低估速度可能变化的程度,转弯时的速度修正便不足,下一拍继续沿旧方向预测;这不意味着误差必然无限增长。

用加速度尺度构造 Q

假设每个采样区间内有一个恒定随机加速度:均值为零,不同区间独立,两个轴独立且标准差同为 σa\sigma_a。按上述状态顺序,位置与速度增量由同一份加速度产生,因此保留它们的互协方差:

G=[(Δt)2/200(Δt)2/2Δt00Δt],Q=σa2GGT.G= \begin{bmatrix} (\Delta t)^2/2 & 0\\ 0 & (\Delta t)^2/2\\ \Delta t & 0\\ 0 & \Delta t \end{bmatrix}, \qquad Q=\sigma_a^2GG^T.

σa\sigma_a 的单位是 m/s2\mathrm{m/s^2},必须先平方再用于 Q。在 Δt=0.1 s\Delta t=0.1\,\mathrm{s} 下,σa=0.02\sigma_a=0.02 与 1.001.00 对应的每轴速度新增方差分别为 0.0000040.000004 与 0.01 m2/s20.01\,\mathrm{m^2/s^2};标准差增大 50 倍,整张 Q 放大 2500 倍。

这是“每拍恒定随机加速度”的离散模型,与用连续白噪声谱密度构造 Q 的模型不同,不能互换参数含义;改变采样间隔也必须重算 G 和 Q。该构造可对照 FilterPy 官方实现说明。genui{"citation":{"ref":"turn3view0"}}

这里的 σa\sigma_a 是对未建模机动强度的假设,不是已知的真实转弯加速度。测量噪声另行固定为 R=0.25I2R=0.25I_2,即每轴位置测量标准差为 0.5 m;调整 Q 时不同时修改 R。

复现直行与转弯的差异

生成 0~20 s、间隔 0.1 s 的 201 个样本:前 6 s 向东直行,中间 6 s 沿四分之一圆弧左转,最后 8 s 向北直行,速率始终为 2 m/s。位置测量只生成一次,三个滤波器共享同一份测量数组。

为隔离 Q 的影响,初速度估计统一取真实起步速度 [2,0]T[2,0]^T,初始位置取首帧测量,P0=diag⁡(0.25,0.25,1,1)P_0=\operatorname{diag}(0.25,0.25,1,1)。真值用于生成测量和评估,不送入后续滤波更新;首帧也不重复更新。

评价使用每拍更新后的后验估计。直行窗口为 2≤t<6 s2\le t<6\,\mathrm{s},避开最初两秒;转弯窗口为 6<t≤12 s6<t\le12\,\mathrm{s}。二维位置 RMSE 定义为 mean⁡((p^x−px)2+(p^y−py)2)\sqrt{\operatorname{mean}((\hat p_x-p_x)^2+(\hat p_y-p_y)^2)},单位 m;速度采用同样的向量误差计算,单位 m/s。

完整脚本在 Python 3.12、NumPy 2.3.5、Matplotlib 3.10.8 下验证,请在支持图形显示的 Python 环境运行。输出中 straight_p、turn_p、turn_v 分别是直行位置、转弯位置、转弯速度的 RMSE,vy_12 是 12 s 时估计的纵向速度。

python
import numpy as np
import matplotlib.pyplot as plt

dt = 0.1
t = np.arange(201) * dt
speed, omega = 2.0, np.pi / 12
theta = omega * np.clip(t - 6.0, 0.0, 6.0)

# 真值连续经过直线、四分之一圆弧、直线,仅用于造数据和评估。
truth = np.zeros((len(t), 4))
truth[:, 0] = speed * np.minimum(t, 6.0) + speed / omega * np.sin(theta)
truth[:, 1] = speed / omega * (1 - np.cos(theta)) + speed * np.maximum(t - 12.0, 0.0)
truth[:, 2] = speed * np.cos(theta)
truth[:, 3] = speed * np.sin(theta)
rng = np.random.Generator(np.random.PCG64(7))
z = truth[:, :2] + rng.normal(0.0, 0.5, (len(t), 2))

# 顺序固定为 px、py、vx、vy;两个位置传感器独立。
F = np.eye(4)
F[0, 2] = F[1, 3] = dt
H = np.eye(4)[:2]
G = np.vstack((0.5 * dt**2 * np.eye(2), dt * np.eye(2)))
R = 0.25 * np.eye(2)

def run(sigma_a):
    Q = sigma_a**2 * (G @ G.T)
    x = np.r_[z[0], 2.0, 0.0]
    P = np.diag([0.25, 0.25, 1.0, 1.0])
    estimates = [x.copy()]
    for k in range(1, len(t)):
        # 首帧已用于初始化;从第二帧起先预测,再处理同一时刻的测量。
        x = F @ x
        P = F @ P @ F.T + Q
        innovation = z[k] - H @ x
        S = H @ P @ H.T + R
        K = np.linalg.solve(S, H @ P).T
        x = x + K @ innovation

        # Joseph 形式保留协方差更新的完整结构。
        A = np.eye(4) - K @ H
        P = A @ P @ A.T + K @ R @ K.T
        estimates.append(x.copy())
    return np.array(estimates)

def rmse(error):
    return np.sqrt(np.mean(np.sum(error**2, axis=1)))

straight = (t >= 2.0) & (t < 6.0)
turn = (t > 6.0) & (t <= 12.0)
curves = {}
print("sigma_a  straight_p  turn_p  turn_v  vy_12")
for sigma_a in (0.02, 1.0, 5.0):
    est = run(sigma_a)  # 三次运行复用同一份 z,只改变 Q。
    curves[sigma_a] = est
    p0 = rmse(est[straight, :2] - truth[straight, :2])
    p1 = rmse(est[turn, :2] - truth[turn, :2])
    v1 = rmse(est[turn, 2:] - truth[turn, 2:])
    print(f"{sigma_a:7.2f}  {p0:10.3f}  {p1:6.3f}  {v1:6.3f}  {est[120, 3]:5.3f}")
print(f"raw_turn_p={rmse(z[turn] - truth[turn, :2]):.3f} m")
print(f"true_vy_12={truth[120, 3]:.3f} m/s")

# 左图看空间偏离,右图看速度跟随;英文标签避免依赖中文字体。
fig, ax = plt.subplots(1, 2, figsize=(12, 6.75))
ax[0].scatter(z[:, 0], z[:, 1], s=5, color="0.8", label="measurements")
ax[0].plot(truth[:, 0], truth[:, 1], color="black", label="truth")
ax[1].plot(t, truth[:, 3], color="black", label="truth")
for sigma_a, color, style in [(0.02, "#D97732", "--"),
                              (1.0, "#168F87", "-"),
                              (5.0, "#75849A", ":")]:
    est = curves[sigma_a]
    label = f"sigma_a={sigma_a:g}"
    ax[0].plot(est[:, 0], est[:, 1], color=color, ls=style, label=label)
    ax[1].plot(t, est[:, 3], color=color, ls=style, label=label)
ax[0].set(xlabel="px (m)", ylabel="py (m)", title="Position")
ax[0].set_aspect("equal", adjustable="box")
ax[1].set(xlabel="t (s)", ylabel="vy (m/s)", title="Velocity response")
ax[1].axvspan(6.0, 12.0, color="#D97732", alpha=0.08)
for axis in ax:
    axis.grid(alpha=0.2)
    axis.legend(fontsize=8)
plt.tight_layout()
plt.show()

预期输出(固定种子 7,保留三位小数):

text
sigma_a  straight_p  turn_p  turn_v  vy_12
   0.02       0.135   2.003   1.468  0.575
   1.00       0.185   0.360   0.528  1.817
   5.00       0.272   0.401   0.793  2.283
raw_turn_p=0.669 m
true_vy_12=2.000 m/s

还会出现一个双栏图窗:左图显示空间轨迹,右图显示纵向速度,淡橙背景标出 6~12 s 的转弯区间。下图突出 σa=0.02\sigma_a=0.02 与 1.001.00 的差别;第三档参数的曲线可在运行结果中比较。

相同测量下,小 Q 造成速度响应滞后和空间轨迹偏离

根据误差定位并调整

小 Q 的直行位置 RMSE 只有 0.135 m,但转弯位置 RMSE 达到 2.003 m,甚至高于原始测量的 0.669 m。到 12 s,真实纵向速度已经是 2 m/s,它却只估到 0.575 m/s:位置观测尚未充分修正旧速度,后续预测继续偏离。

将 σa\sigma_a 增至 1.00 后,转弯位置 RMSE 降到 0.360 m,速度 RMSE 从 1.468 降到 0.528 m/s;代价是直行位置 RMSE 从 0.135 升到 0.185 m。本例中允许更多速度变化,有助于响应机动。

继续增至 5.00,直行位置 RMSE 升到 0.272 m,转弯速度 RMSE 也升到 0.793 m/s。更大的 Q 让测量噪声更容易进入估计,不能只追求“转向更快”。这三档只展示调参方向,不能证明 1.00 是最优参数。

遇到类似拖尾,可按以下顺序处理:

  1. 核对时间和状态顺序:100 ms 应写成 0.1 s,F、G、H 都必须匹配 [px, py, vx, vy]。时间戳错位也会表现为滞后。
  2. 检查 R 是否有测量依据。每轴标准差 0.5 m 对应方差 0.25,不应为了跟手而随意降低 R,也不能把改变初始 P 当作持续过程噪声的替代。
  3. 固定测量后比较 σa=0.02,0.1,0.5,1,2,5\sigma_a=0.02,0.1,0.5,1,2,5,再把随机种子依次改为 0~19 重跑,分别汇总直行和转弯误差。若需要一致性结论,还应检查新息统计与协方差覆盖情况;单次 RMSE 更小并不充分。

持续转弯包含有方向、随时间连续变化的加速度。增大 Q 能让测量更快纠正匀速预测,但下一拍仍沿直线外推;若多个测试轨迹都需要很大的 Q 才能勉强跟上,同时直行抖动明显增加,应考虑增加加速度状态或采用转弯模型。

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

目录与阅读设置

阅读设置

17

记录一个想法

过程噪声 Q 过小为何跟不上转弯

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

登录哪都通