自动控制 / 系统课时

根据残差定位滤波发散原因

根据残差定位滤波发散原因

位置估计突然滞后、抖动或偏离真值时,只看滤波轨迹难以分清运动模型失配、传感器偏置和噪声参数错误。使用一维位置—速度模型模拟三类故障,联合新息、NIS、协方差与状态误差进行诊断;二维跟踪也可沿相同思路排查,但联合观测要按观测维数调整统计检验。

学习目标

  • 正确记录更新前新息、创新协方差与 NIS,避免将后验残差当成诊断输入。
  • 从时序曲线区分突然机动、持续测量偏置以及 Q、R 设置不当。
  • 运行固定随机种子的实验,依据可复核的指标修改参数并验收。

先记录能反映故障的四个量

假设状态为位置和速度 xk=[pk,vk]Tx_k=[p_k,v_k]^T,只观测位置,H=[1,0]H=[1,0]。每拍先预测,再在观测更新前记录以下量,不能拿更新后的状态回算新息:

  • 新息:νk=zk−Hx^k∣k−1\nu_k=z_k-H\hat{x}_{k|k-1}。持续同号通常意味着预测存在系统性偏差;孤立尖峰可能是异常观测,也可能是真实机动。
  • 新息方差:Sk=HPk∣k−1HT+RS_k=HP_{k|k-1}H^T+R。它把模型预测的不确定性与测量噪声相加,必须大于零。
  • 归一化新息平方:NISk=νk2/Sk\mathrm{NIS}_k=\nu_k^2/S_k。在线性高斯且参数匹配的单维观测假设下,单点 95% 上界约为 3.84;一次越界不能直接判为发散。二维联合位置观测应改用 νkTSk−1νk\nu_k^TS_k^{-1}\nu_k,其 95% 单点门限约为 5.99,而不是 3.84。
  • 后验位置误差及区间:ek=p^k∣k−pktruee_k=\hat{p}_{k|k}-p_k^{true},对照 ±2Pk∣k[0,0]\pm2\sqrt{P_{k|k}[0,0]}。真值只在仿真或有独立参考设备时可得,线上不能凭滤波输出臆造真值。

实际日志至少保留采样时间戳、原始观测、预测位置、新息、SkS_k、NIS、后验位置及 Pk∣k[0,0]P_{k|k}[0,0]。窗口内还应看新息均值、连续同号现象和越界比例;仅看某个最大值容易把转弯错诊成传感器坏点。

三类异常会留下不同的时序指纹

情形 更新前新息与 NIS 位置误差和报告的不确定性 优先核查
速度突然增加,Q 合理 正向新息短时升高,随后回落 位置短暂滞后后追上 是否发生真实机动
速度突然增加,Q 过小 新息连续为正,NIS 长时间偏高 估计落后真值,但 PP 仍很小 过程噪声、运动模型
传感器在途中增加 +5 m 偏置 新息先出现正向尖峰,滤波器吸收偏置后可能回到零附近 位置误差长期约 +5 m,PP 未必变大 标定、第二传感器
R 被设置得过小 新息随噪声正负波动,NIS 频繁越界 估计追随噪声,PP 报告的区间过窄 测量方差和单位

三类滤波故障的新息与误差时序对比

注意,新息趋近零不等于位置估计无偏。恒定传感器偏置会逐渐被滤波器当作真实位置变化吸收,因此诊断需要独立参考,不能只检查 NIS 是否回到正常范围。

用固定数据复现并量化故障

在 Python 3.10+ 环境安装 NumPy 后运行以下完整脚本。采样间隔为 1 s,真实测量噪声标准差为 1 m;速度在索引 30 从 1 m/s 跃变至 3 m/s。采用 Q=qGGTQ=qGG^T,其中 G=[0.5,1]TG=[0.5,1]^T;正常取 q=0.2q=0.2、R=1R=1,故障分别改为 q=0.0001q=0.0001 或 R=0.04R=0.04,单位需与状态定义一致。程序使用 Joseph 形式更新协方差,比较故障后的 30 次观测。

python
import numpy as np

n = 60
noise = np.random.default_rng(2026).normal(0, 1, n)
normal = np.arange(1.0, n + 1)
# 索引30起从每拍前进1米变为3米,模拟未建模的机动。
jump = normal + 2.0 * np.maximum(np.arange(n) - 29, 0)
F = np.array([[1., 1.], [0., 1.]])
H = np.array([1., 0.])
G = np.array([0.5, 1.0])

def diagnose(truth, z_values, q, r):
    x = np.array([0., 1.])
    P = np.diag([4., 1.])
    innovations, nis_values, errors, sigmas = [], [], [], []
    for k, z in enumerate(z_values):
        x = F @ x
        P = F @ P @ F.T + q * np.outer(G, G)
        # 诊断必须使用更新前的新息,而非更新后的残差。
        nu = float(z - H @ x)
        S = float(H @ P @ H + r)
        K = P @ H / S
        x = x + K * nu
        A = np.eye(2) - np.outer(K, H)
        P = A @ P @ A.T + r * np.outer(K, K)
        innovations.append(nu)
        nis_values.append(nu**2 / S)
        errors.append(x[0] - truth[k])
        sigmas.append(np.sqrt(P[0, 0]))
    # 只比较故障发生后的30拍;真实位置仅用于离线验证。
    s = slice(30, None)
    nu = np.array(innovations)[s]
    nis = np.array(nis_values)[s]
    err = np.array(errors)[s]
    sigma = np.array(sigmas)[s]
    return (nu.mean(), nis.mean(), np.mean(nis > 3.84),
            np.sqrt(np.mean(err**2)), np.mean(2 * sigma),
            np.mean(np.abs(err) > 2 * sigma))

cases = [
    ('baseline', normal, normal + noise, 0.2, 1.),
    ('jump-q-ok', jump, jump + noise, 0.2, 1.),
    ('jump-q-low', jump, jump + noise, 0.0001, 1.),
    ('bias+5m', normal, normal + noise + 5*(np.arange(n) >= 30), 0.2, 1.),
    ('r-too-low', normal, normal + noise, 0.2, 0.04),
]
print('case          nu    NIS   >3.84  RMSE  2sig  >2sig')
for name, truth, z, q, r in cases:
    mean_nu, mean_nis, high, rmse, bound, outside = diagnose(truth, z, q, r)
    print(f'{name:12s} {mean_nu:5.2f} {mean_nis:6.2f} {high:6.0%} '
          f'{rmse:5.2f} {bound:5.2f} {outside:6.0%}')

预期输出(固定随机种子,NumPy 浮点结果按上述格式保留两位):

text
case          nu    NIS   >3.84  RMSE  2sig  >2sig
baseline      0.01   0.81     0%  0.95  1.56    13%
jump-q-ok     0.25   1.08     3%  0.96  1.56    17%
jump-q-low    6.81  46.75    93%  5.82  0.73   100%
bias+5m       0.01   1.25    10%  5.56  1.56   100%
r-too-low    -0.07  13.39    57%  1.08  0.37    70%

列名中,nu 是故障后新息均值;NIS 是平均 NIS;>3.84 是单点越界比例;RMSE 为位置误差均方根(米);2sig 是模型报告的平均双标准差半宽(米);>2sig 是真实位置误差落在所报区间之外的比例。它们都是最后 30 拍的统计量,不代表任何真实传感器的通用阈值。

对照数值区分根因

同一条机动轨迹下,qq 从 0.0001 改为 0.2 后,平均 NIS 从 46.75 降至 1.08,RMSE 从 5.82 m 降至 0.96 m;这是运动模型过度自信的直接对照,而不是通过平滑输出掩盖问题。

观测突然增加 5 m 偏置后,故障窗口新息均值却只有 0.01 m,位置 RMSE 达到 5.56 m,100% 样本落在报告的双标准差区间外。滤波器吸收稳定偏置后,新息检验会失去辨识力,必须用独立位置基准或冗余传感器验证。

当 RR 错设为 0.04 而真实测量方差为 1 时,平均 NIS 升至 13.39,报告的 2sig 缩到 0.37 m。此时滤波器过度相信观测,协方差看起来更小,却不能证明估计更准确。短窗口里的 13% 或 0% 越界率也不宜机械地当作合格或不合格结论。

按顺序修复并验证

  1. 先排除数据错误:核查位置单位是否统一为米、采样是否按时间戳排序、实际 Δt\Delta t 是否用于状态转移矩阵,以及观测矩阵 HH 是否选中了正确轴;这些错误不能靠增大 Q 补救。
  2. 再分开验证传感器和模型:用静止或已知轨迹的独立参考估计测量偏置及 RR;在真实加减速片段检查运动模型与 QQ。不要把含真实运动趋势的原始读数方差直接当作 RR。
  3. 一次只改一个原因:在脚本里将 jump-q-low 的 q 恢复为 0.2,应回到 jump-q-ok 那组指标;将 r-too-low 的 r 恢复为 1.0,应回到 baseline;将 bias+5m 的额外 5 去掉,也应与 baseline 一致。
  4. 复核整段记录:观察新息是否长期单侧偏移、NIS 是否频繁越界,检查 PP 是否对称且半正定;有真值时再看 RMSE 与置信区间覆盖率。若协方差出现数值异常,优先检查矩阵维度、噪声协方差的半正定性(RR 应正定)及 Joseph 更新,而不是仅调整阈值。

当 NIS 持续异常时,先判断是预测均值错、预测不确定性错,还是测量本身偏了。三者可能产生相似的残差,最终归因必须依靠时间对齐的数据、受控参数对照以及独立观测证据。

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

目录与阅读设置

阅读设置

17

记录一个想法

根据残差定位滤波发散原因

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

登录哪都通