自动控制 / 系统课时
过程噪声 Q 过小为何跟不上转弯
过程噪声 Q 过小为何跟不上转弯
小车直行时估计很平滑,一转弯却沿旧方向拖行,可能是匀速模型的过程噪声设得过小。通过同一组位置测量和三档 Q,你将复现速度修正不足、位置误差扩大,以及增大 Q 后响应改善但抖动增加的现象。
学习目标
- 解释速率不变的转弯为何仍会破坏匀速模型假设。
- 按状态顺序和 构造过程噪声协方差。
- 比较三档 Q 的直行、转弯误差,区分参数过小与运动模型不适用。
转弯怎样打破匀速假设
状态按 排列,前两项是位置,后两项是速度。匀速模型让位置按当前速度外推,同时保持两个速度分量不变;因此,速率同为 ,由向东运动改成向北运动,依然是模型没有预测到的变化。
本例在 6 s 开始左转,经过 6 s 转过 90°,速度向量从 变成 。滤波器只收到位置测量,不知道转弯开始时间,也没有转向控制输入;转向后的新位置必须通过测量更新逐步改变速度估计。
设 为上拍后验协方差, 提取两个位置分量, 为测量噪声协方差。预测均值仍为 ,Q 通过以下关系影响修正力度:
Q 不直接向预测均值添加随机扰动。虽然传感器只读位置,P 中的位置与速度互协方差仍能让位置新息修正速度。直行阶段反复更新后,若 Q 太小,滤波器会低估速度可能变化的程度,转弯时的速度修正便不足,下一拍继续沿旧方向预测;这不意味着误差必然无限增长。
用加速度尺度构造 Q
假设每个采样区间内有一个恒定随机加速度:均值为零,不同区间独立,两个轴独立且标准差同为 。按上述状态顺序,位置与速度增量由同一份加速度产生,因此保留它们的互协方差:
的单位是 ,必须先平方再用于 Q。在 下, 与 对应的每轴速度新增方差分别为 与 ;标准差增大 50 倍,整张 Q 放大 2500 倍。
这是“每拍恒定随机加速度”的离散模型,与用连续白噪声谱密度构造 Q 的模型不同,不能互换参数含义;改变采样间隔也必须重算 G 和 Q。该构造可对照 FilterPy 官方实现说明。genui{"citation":{"ref":"turn3view0"}}
这里的 是对未建模机动强度的假设,不是已知的真实转弯加速度。测量噪声另行固定为 ,即每轴位置测量标准差为 0.5 m;调整 Q 时不同时修改 R。
复现直行与转弯的差异
生成 0~20 s、间隔 0.1 s 的 201 个样本:前 6 s 向东直行,中间 6 s 沿四分之一圆弧左转,最后 8 s 向北直行,速率始终为 2 m/s。位置测量只生成一次,三个滤波器共享同一份测量数组。
为隔离 Q 的影响,初速度估计统一取真实起步速度 ,初始位置取首帧测量,。真值用于生成测量和评估,不送入后续滤波更新;首帧也不重复更新。
评价使用每拍更新后的后验估计。直行窗口为 ,避开最初两秒;转弯窗口为 。二维位置 RMSE 定义为 ,单位 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 时估计的纵向速度。
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,保留三位小数):
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 的转弯区间。下图突出 与 的差别;第三档参数的曲线可在运行结果中比较。

根据误差定位并调整
小 Q 的直行位置 RMSE 只有 0.135 m,但转弯位置 RMSE 达到 2.003 m,甚至高于原始测量的 0.669 m。到 12 s,真实纵向速度已经是 2 m/s,它却只估到 0.575 m/s:位置观测尚未充分修正旧速度,后续预测继续偏离。
将 增至 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 是最优参数。
遇到类似拖尾,可按以下顺序处理:
- 核对时间和状态顺序:100 ms 应写成 0.1 s,F、G、H 都必须匹配
[px, py, vx, vy]。时间戳错位也会表现为滞后。 - 检查 R 是否有测量依据。每轴标准差 0.5 m 对应方差 0.25,不应为了跟手而随意降低 R,也不能把改变初始 P 当作持续过程噪声的替代。
- 固定测量后比较 ,再把随机种子依次改为 0~19 重跑,分别汇总直行和转弯误差。若需要一致性结论,还应检查新息统计与协方差覆盖情况;单次 RMSE 更小并不充分。
持续转弯包含有方向、随时间连续变化的加速度。增大 Q 能让测量更快纠正匀速预测,但下一拍仍沿直线外推;若多个测试轨迹都需要很大的 Q 才能勉强跟上,同时直行抖动明显增加,应考虑增加加速度状态或采用转弯模型。