自动控制 / 系统课时

测量噪声 R 过小为何引入抖动

测量噪声 R 过小为何引入抖动

把 R 调小后,估计曲线可能迅速贴近传感器读数,却把读数里的随机变化带进位置和速度。本文固定运动模型与测量样本,比较三种 R 的增益、抖动和真实误差,并用一段可运行的 Python 程序验证调参结果。

学习目标

  • 区分测量标准差与方差,正确把 0.5 m 的标准差换算为 R。
  • 复现只改变 R 的对照实验,比较测量权重、位置抖动和位置、速度误差。
  • 结合静止采样与新息统计,判断 R 是否低估了测量噪声。

R 怎样把测量噪声带入估计

只测位置时,R 是位置测量噪声的方差,单位为平方米。若真实标准差为 0.5 m,则真实方差是 0.25 m²。填入 0.0025 m²,相当于假设标准差只有 0.05 m:标准差低估了 10 倍,方差低估了 100 倍。

令状态为 x=[p,v]Tx=[p,v]^T,观测矩阵为 H=[1,0]H=[1,0]。上标 −- 表示测量更新前,上标 ++ 表示更新后。位置增益与位置更新为:

Kp=Ppp−Ppp−+R,p^+=p^−+Kp(z−p^−)K_p=\frac{P_{pp}^-}{P_{pp}^-+R},\qquad \hat p^+=\hat p^-+K_p(z-\hat p^-)

固定同一份先验 Ppp−=0.25 m2P_{pp}^-=0.25\ \mathrm{m^2},令预测位置为 10 m、测量为 11 m。采用 R=0.25R=0.25 时,Kp=0.5K_p=0.5,位置修正到 10.5 m;采用 R=0.0025R=0.0025 时,Kp≈0.9901K_p\approx0.9901,位置修正到约 10.9901 m。

读数中的噪声也经过这次修正进入估计。位置与速度存在先验交叉协方差时,同一新息还会修正速度,所以只测位置也可能出现速度抖动。标准增益与更新关系可核对 Welch 与 Bishop 的原文。genui{"citation":{"ref":"turn2view0"}}

测量噪声经新息与增益进入状态估计,R 只参与不确定性与权重计算

完整递推中,改变 R 也会改变后续先验协方差。因此,上面的 0.9901 只是固定先验的一步算例;长期实验要实际记录增益,不能把它当作全程不变的权重。

固定模型和同一组测量

实验每 0.1 s 采样一次,共 1000 个时刻,覆盖 0~99.9 s。状态按位置、速度排列。真实运动以匀速传播为基础,每个采样区间叠加独立的随机加速度,标准差为 0.3 m/s²,区间内加速度保持常数。

滤波器使用相同的运动统计:F=[10.101]F=\begin{bmatrix}1&0.1\\0&1\end{bmatrix},G=[0.005,0.1]TG=[0.005,0.1]^T,Q=0.32GGTQ=0.3^2GG^T。位置测量另加独立的零均值高斯噪声,标准差固定为 0.5 m。

三组共用真值、读数、F、H、Q 和初值。每次都从位置 0 m、速度 1 m/s,以及对角元素为 1、1 的初始协方差开始;两个方差的单位分别为 m²、(m/s)²。只把 R 改为 0.0025、0.25、25 m²,并统一排除前 200 拍的启动过程。

特别注意:生成噪声时,rng.normal 的第二个参数是标准差,因此传入 0.5;滤波更新使用的真实方差基准才是 0.25。参数定义见 NumPy 文档。genui{"citation":{"ref":"turn1view0"}}

运行对照并观察误差曲线

环境需要 Python 3、NumPy 和 Matplotlib;下列输出在 NumPy 2.3.5、Matplotlib 3.10.8 下核对。将完整程序作为 compare_r.py 运行,或粘贴到支持绘图的 Notebook 单元格中。

令评估区间的位置误差为 ej=p^j−pje_j=\hat p_j-p_j,样本数 M=800M=800。位置 RMSE 衡量偏离真值的总体大小;本例另定义抖动指标 JJ,衡量误差的逐拍变化:

RMSEp=1M∑j=1Mej2,J=1M−1∑j=2M(ej−ej−1)2\mathrm{RMSE}_p=\sqrt{\frac{1}{M}\sum_{j=1}^{M}e_j^2},\qquad J=\sqrt{\frac{1}{M-1}\sum_{j=2}^{M}(e_j-e_{j-1})^2}

J 的单位仍是米,因为没有除以采样周期;它不是速度 RMSE。先减去真值,可以避免把真实运动增量本身算成抖动。真值只用于生成仿真读数和事后评估,滤波循环只接收测量;最后一列的新息指标在后文解释。

python
import numpy as np
import matplotlib.pyplot as plt

dt, n, burn = 0.1, 1000, 200
F = np.array([[1.0, dt], [0.0, 1.0]])
H = np.array([1.0, 0.0])
G = np.array([0.5 * dt**2, dt])
sigma_a, sigma_z = 0.3, 0.5
Q = sigma_a**2 * np.outer(G, G)
rng = np.random.Generator(np.random.PCG64(2026))

# 真值和读数仅生成一次;R 不参与测量噪声的生成。
truth = np.zeros((n, 2))
truth[0] = [0.0, 1.0]
for k in range(1, n):
    truth[k] = F @ truth[k - 1] + G * rng.normal(0.0, sigma_a)
z = truth[:, 0] + rng.normal(0.0, sigma_z, n)

def run_filter(R):
    # 每组恢复相同初值;t=0 直接更新,不额外预测一拍。
    x, P = np.array([0.0, 1.0]), np.diag([1.0, 1.0])
    estimates, gains, nis = [], [], []
    for k, zk in enumerate(z):
        if k > 0:
            x = F @ x
            P = F @ P @ F.T + Q
        y = float(zk - H @ x)
        S = float(H @ P @ H + R)
        K = P @ H / S
        x = x + K * y
        # Joseph 形式更新协方差,降低浮点舍入的影响。
        A = np.eye(2) - np.outer(K, H)
        P = A @ P @ A.T + R * np.outer(K, K)
        estimates.append(x.copy())
        gains.append(K[0])
        nis.append(y**2 / S)
    return np.array(estimates), np.array(gains), np.array(nis)

# 统一去掉前20秒;J衡量误差的逐拍变化,单位为米。
t = np.arange(n) * dt
fig, axes = plt.subplots(3, 1, sharex=True, sharey=True, figsize=(9, 7))
raw_rmse = np.sqrt(np.mean((z[burn:] - truth[burn:, 0])**2))
print(f'measurement_RMSE_m = {raw_rmse:.3f}')
print('R mean_Kp pos_RMSE_m vel_RMSE_mps J_m mean_NIS')
for ax, R in zip(axes, [0.0025, 0.25, 25.0]):
    estimates, gains, nis = run_filter(R)
    error = estimates - truth
    rmse = np.sqrt(np.mean(error[burn:]**2, axis=0))
    jitter = np.sqrt(np.mean(np.diff(error[burn:, 0])**2))
    print(f'{R:.4f} {gains[burn:].mean():.3f} {rmse[0]:.3f} '
          f'{rmse[1]:.3f} {jitter:.3f} {nis[burn:].mean():.3f}')
    # 相同时间窗和纵轴,减去真值后可看清估计误差的抖动。
    ax.plot(t, z - truth[:, 0], '.', color='#dd9045', alpha=0.5,
            label='measurement error')
    ax.plot(t, error[:, 0], color='#248d87', label='estimate error')
    ax.axhline(0.0, color='#364654', linewidth=0.8)
    ax.set(xlim=(20, 25), ylim=(-1.5, 1.5), ylabel='error (m)',
           title=f'R = {R:g} m^2')
    ax.grid(alpha=0.2)
axes[0].legend(loc='upper right')
axes[-1].set_xlabel('time (s)')
fig.tight_layout()
plt.show()

预期输出

text
measurement_RMSE_m = 0.501
R mean_Kp pos_RMSE_m vel_RMSE_mps J_m mean_NIS
0.0025 0.292 0.241 0.364 0.156 91.898
0.2500 0.104 0.159 0.131 0.054 1.005
25.0000 0.034 0.277 0.140 0.020 0.012

同时显示三幅共享坐标尺度的误差图,只观察 20~25 s:橙色点是同一组测量误差,青绿色线是估计误差,零线表示估计等于真值。三组的橙色点应完全相同;统计量则使用 20~99.9 s 的全部 800 个时刻。

怎样判断调参是否有效

本次结果中,小 R 组的平均位置增益为 0.292,正常组为 0.104。小 R 组的 J 从正常组的 0.054 m 增至 0.156 m,位置 RMSE 从 0.159 m 增至 0.241 m,速度 RMSE 也从 0.131 m/s 增至 0.364 m/s。

大 R 组的 J 降到 0.020 m,曲线更平滑,但位置 RMSE 升到 0.277 m。它弱化了测量修正,对真实随机加速度造成的位置变化响应不足。因此,验收时要同时看抖动和真实误差;单个随机种子的排名也不等于所有工况的保证。

代码还记录了归一化新息平方 NIS=y2/S\mathrm{NIS}=y^2/S,其中 y=z−Hx^−y=z-H\hat x^-、S=HP−HT+RS=HP^-H^T+R。对单个位置观测,若新息零均值且方差确为 S,则 NIS 的期望为 1,有限样本均值不必恰好等于 1。

小 R 组的平均 NIS 约为 91.898,表示实际偏差远大于算法给出的预测尺度;正常组约为 1.005。NIS 持续偏大也可能来自 Q 过小、模型失配、测量偏置或时间未对齐,不能仅凭这一列断定 R 有误。

用传感器统计确定 R 的起点

先在与实际运行相同的采样率、量程和预处理设置下固定目标,采集例如 1000 个位置读数。对这些读数减去样本均值,再计算样本方差;目标必须静止,否则运动变化也会被混进噪声估计。

R^=1M−1∑i=1M(zi−zˉ)2\hat R=\frac{1}{M-1}\sum_{i=1}^{M}(z_i-\bar z)^2

若得到标准差约 0.5 m,就以 0.25 m² 为起点。保留另一段有真值的运动记录做验证,同时检查位置、速度 RMSE 与 J;没有真值时可检查新息均值和 NIS,但不能直接计算本文的真实误差指标。

遇到转弯或加速时明显滞后,应检查运动模型与 Q 是否允许这种变化,再结合传感器统计判断 R。固定偏差需要校准或增加偏置状态,时间错位需要先对齐;只调 R 无法充分解释这些误差来源。

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

目录与阅读设置

阅读设置

17

记录一个想法

测量噪声 R 过小为何引入抖动

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

登录哪都通