自动控制 / 系统课时
根据残差定位滤波发散原因
根据残差定位滤波发散原因
位置估计突然滞后、抖动或偏离真值时,只看滤波轨迹难以分清运动模型失配、传感器偏置和噪声参数错误。使用一维位置—速度模型模拟三类故障,联合新息、NIS、协方差与状态误差进行诊断;二维跟踪也可沿相同思路排查,但联合观测要按观测维数调整统计检验。
学习目标
- 正确记录更新前新息、创新协方差与 NIS,避免将后验残差当成诊断输入。
- 从时序曲线区分突然机动、持续测量偏置以及 Q、R 设置不当。
- 运行固定随机种子的实验,依据可复核的指标修改参数并验收。
先记录能反映故障的四个量
假设状态为位置和速度 ,只观测位置,。每拍先预测,再在观测更新前记录以下量,不能拿更新后的状态回算新息:
- 新息:。持续同号通常意味着预测存在系统性偏差;孤立尖峰可能是异常观测,也可能是真实机动。
- 新息方差:。它把模型预测的不确定性与测量噪声相加,必须大于零。
- 归一化新息平方:。在线性高斯且参数匹配的单维观测假设下,单点 95% 上界约为 3.84;一次越界不能直接判为发散。二维联合位置观测应改用 ,其 95% 单点门限约为 5.99,而不是 3.84。
- 后验位置误差及区间:,对照 。真值只在仿真或有独立参考设备时可得,线上不能凭滤波输出臆造真值。
实际日志至少保留采样时间戳、原始观测、预测位置、新息、、NIS、后验位置及 。窗口内还应看新息均值、连续同号现象和越界比例;仅看某个最大值容易把转弯错诊成传感器坏点。
三类异常会留下不同的时序指纹
| 情形 | 更新前新息与 NIS | 位置误差和报告的不确定性 | 优先核查 |
|---|---|---|---|
| 速度突然增加,Q 合理 | 正向新息短时升高,随后回落 | 位置短暂滞后后追上 | 是否发生真实机动 |
| 速度突然增加,Q 过小 | 新息连续为正,NIS 长时间偏高 | 估计落后真值,但 仍很小 | 过程噪声、运动模型 |
| 传感器在途中增加 +5 m 偏置 | 新息先出现正向尖峰,滤波器吸收偏置后可能回到零附近 | 位置误差长期约 +5 m, 未必变大 | 标定、第二传感器 |
| R 被设置得过小 | 新息随噪声正负波动,NIS 频繁越界 | 估计追随噪声, 报告的区间过窄 | 测量方差和单位 |

注意,新息趋近零不等于位置估计无偏。恒定传感器偏置会逐渐被滤波器当作真实位置变化吸收,因此诊断需要独立参考,不能只检查 NIS 是否回到正常范围。
用固定数据复现并量化故障
在 Python 3.10+ 环境安装 NumPy 后运行以下完整脚本。采样间隔为 1 s,真实测量噪声标准差为 1 m;速度在索引 30 从 1 m/s 跃变至 3 m/s。采用 ,其中 ;正常取 、,故障分别改为 或 ,单位需与状态定义一致。程序使用 Joseph 形式更新协方差,比较故障后的 30 次观测。
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 浮点结果按上述格式保留两位):
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 拍的统计量,不代表任何真实传感器的通用阈值。
对照数值区分根因
同一条机动轨迹下, 从 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% 样本落在报告的双标准差区间外。滤波器吸收稳定偏置后,新息检验会失去辨识力,必须用独立位置基准或冗余传感器验证。
当 错设为 0.04 而真实测量方差为 1 时,平均 NIS 升至 13.39,报告的 2sig 缩到 0.37 m。此时滤波器过度相信观测,协方差看起来更小,却不能证明估计更准确。短窗口里的 13% 或 0% 越界率也不宜机械地当作合格或不合格结论。
按顺序修复并验证
- 先排除数据错误:核查位置单位是否统一为米、采样是否按时间戳排序、实际 是否用于状态转移矩阵,以及观测矩阵 是否选中了正确轴;这些错误不能靠增大 Q 补救。
- 再分开验证传感器和模型:用静止或已知轨迹的独立参考估计测量偏置及 ;在真实加减速片段检查运动模型与 。不要把含真实运动趋势的原始读数方差直接当作 。
- 一次只改一个原因:在脚本里将
jump-q-low的q恢复为0.2,应回到jump-q-ok那组指标;将r-too-low的r恢复为1.0,应回到baseline;将bias+5m的额外5去掉,也应与baseline一致。 - 复核整段记录:观察新息是否长期单侧偏移、NIS 是否频繁越界,检查 是否对称且半正定;有真值时再看 RMSE 与置信区间覆盖率。若协方差出现数值异常,优先检查矩阵维度、噪声协方差的半正定性( 应正定)及 Joseph 更新,而不是仅调整阈值。
当 NIS 持续异常时,先判断是预测均值错、预测不确定性错,还是测量本身偏了。三者可能产生相似的残差,最终归因必须依靠时间对齐的数据、受控参数对照以及独立观测证据。