自动控制 / 系统课时
用马氏距离排除离群测量
用马氏距离排除离群测量
位置传感器偶尔跳出一个尖峰,直接更新可能同时拉偏位置和速度。本篇用新息的马氏距离平方判断观测与预测是否一致,在更新前拒绝异常读数,并用同一组带噪测量验证轨迹误差和后续恢复情况。
学习目标
- 用新息及其协方差计算马氏距离平方,为一维位置观测选择 99% 统计门限。
- 实现接收与拒绝两条分支,拒绝时保留当前预测状态和预测协方差。
- 对比包含两次离群测量的完整轨迹,检查拒绝位置、恢复更新和位置 RMSE。
把位置偏差换成无量纲分数
设状态为位置和速度 ,传感器只测位置,因此 。记 为当前时刻更新前的状态预测, 为对应误差协方差, 为测量噪声协方差;标量位置观测中, 就是测量方差。
先计算新息,也就是实际读数减去预测读数;再把预测误差与测量噪声投影到同一个测量空间:
用于门控的分数是马氏距离平方,也称归一化新息平方 NIS。验证门采用下面的二次型;计算依据是更新前的新息,而不是被当前观测修正后的残差。
一维时就是 ,没有单位。比如预测位置为 10 m,预测位置方差为 4 m²,测量方差为 1 m²,则 m²。若读数为 18 m,新息为 8 m,得到 。分母若只用 ,就漏掉了预测自身的不确定性。
按测量维数选择统计门限
如果未筛选的新息服从零均值高斯分布,且其真实协方差就是 ,白化后的平方和服从自由度为测量维数 的卡方分布。取门限 ,只在 时接收。
| 测量内容 | 测量维数 | 95% 门限 | 99% 门限 |
|---|---|---|---|
| 一个位置读数 | 1 | 3.841 | 6.635 |
| 二维位置读数 | 2 | 5.991 | 9.210 |
表中数值来自 NIST 卡方分布临界值表。本例状态虽然有位置、速度两个分量,但观测只有一个分量,所以用 6.635。若代码计算的是距离 而非平方 ,对应门限应为 。
前面的 ,因此拒绝该观测。若同样偏差对应的 增大到 20 m²,分数就降为 3.2,可以通过;门控考虑的是偏差相对于不确定性的大小,不能替换成固定米数的跳变判断。
拒绝观测时仍然推进预测
每拍先按运动模型得到 和 ,再计算新息与门限分数。上标 表示本拍处理后的结果。接收后才计算增益并更新均值、协方差;拒绝时执行 、,下一拍继续从这里预测。

不要把异常读数替换成预测位置再照常更新:这样虽然新息变成 0,协方差仍会缩小,等于凭空增加了信息。也不要退回上一拍后验;例如拒绝第 12 秒的读数后,状态仍须代表第 12 秒,而不是第 11 秒。
用同一记录对比两种滤波
下例已在 Python 3.12、NumPy 2.3 运行验证。保存为 gate_demo.py,在已安装 NumPy 的环境运行 python gate_demo.py。真值以 1 m/s 匀速运动,共采集 40 个位置读数,间隔 1 s;测量标准差为 0.5 m,第 12、27 次读数分别叠加 8 m、−7 m。
初始估计为位置 0 m、速度 1 m/s,初始标准差分别为 0.5 m、0.3 m/s。滤波器用每步随机加速度标准差 0.05 m/s² 构造 ,其中 ,为未建模运动预留不确定性。此处真值本身没有加入过程扰动。
代码以 np.inf 关闭门控形成对照,两次运行共享同一测量数组。np.linalg.solve 求解线性方程,直接算出 ,不显式构造逆矩阵;这里 为正定矩阵,满足该接口对可解方阵的要求。
import numpy as np
dt = 1.0
F = np.array([[1.0, dt], [0.0, 1.0]])
H = np.array([[1.0, 0.0]])
R = np.array([[0.5**2]])
g = np.array([0.5 * dt**2, dt])
Q = 0.05**2 * np.outer(g, g)
gate = 6.635
# 真值只用于生成测量和验收,不传入滤波函数。
t = np.arange(1, 41, dtype=float) * dt
truth = t
rng = np.random.default_rng(7)
z = truth + rng.normal(0.0, 0.5, t.size)
z[11] += 8.0
z[26] -= 7.0
def run(measurements, threshold):
x = np.array([0.0, 1.0])
P = np.diag([0.25, 0.09])
I = np.eye(2)
history, scores, accepted = [], [], []
for zk in measurements:
# 先推进到当前时刻,再用同一份先验计算门限统计量。
xp = F @ x
Pp = F @ P @ F.T + Q
nu = np.array([zk]) - H @ xp
S = H @ Pp @ H.T + R
d2 = float(nu @ np.linalg.solve(S, nu))
keep = d2 <= threshold
if keep:
K = np.linalg.solve(S, H @ Pp).T
x = xp + K @ nu
# Joseph 形式更新协方差,再对称化以减小舍入影响。
A = I - K @ H
P = A @ Pp @ A.T + K @ R @ K.T
P = 0.5 * (P + P.T)
else:
# 拒绝测量仍保留本步预测,不能退回上一时刻。
x, P = xp, Pp
history.append(x.copy())
scores.append(d2)
accepted.append(keep)
return np.array(history), np.array(scores), np.array(accepted)
plain, _, _ = run(z, np.inf)
gated, nis, accepted = run(z, gate)
bad_steps = (np.flatnonzero(~accepted) + 1).tolist()
print("rejected_steps:", bad_steps)
for step in (12, 13, 27, 28):
i = step - 1
print(f"step={step:02d} d2={nis[i]:.3f} accepted={bool(accepted[i])} "
f"p_plain={plain[i, 0]:.3f} p_gated={gated[i, 0]:.3f}")
for name, states in (("plain", plain), ("gated", gated)):
rmse = np.sqrt(np.mean((states[:, 0] - truth)**2))
print(f"{name}_position_rmse={rmse:.3f} m")
assert bad_steps == [12, 27]
print("checks_passed")预期输出:
rejected_steps: [12, 27]
step=12 d2=168.292 accepted=False p_plain=15.006 p_gated=12.045
step=13 d2=0.001 accepted=True p_plain=15.369 p_gated=13.062
step=27 d2=171.635 accepted=False p_plain=23.879 p_gated=26.931
step=28 d2=0.148 accepted=True p_plain=25.516 p_gated=27.872
plain_position_rmse=1.071 m
gated_position_rmse=0.288 m
checks_passed接收分支用 Joseph 形式更新协方差,随后对称化以消除浮点舍入造成的小误差。固定为两维状态、一维观测时,处理 个读数的时间为 ;示例保存全部轨迹也需 空间,在线只保留当前状态与协方差可使用 空间。
验收轨迹并定位连续拒绝
首先检查拒绝编号是否为 12、27,再检查第 13、28 次正常观测是否恢复接收。第 12 秒真值为 12 m,无门控估计跳到 15.006 m,门控估计为 12.045 m;第 13 秒门控估计继续走到 13.062 m,说明时间传播没有冻结。
位置 RMSE 按全部 40 个时刻计算,包括被拒绝观测对应的时刻,得到 1.071 m 和 0.288 m。不能先删除异常时刻再比较,否则会掩盖轨迹受损程度。这组固定样本验证了分支与恢复行为,不用于验证理论上的 1% 误拒率。
接入真实数据时,先把 NaN、无穷大等非有限读数转入缺测处理,执行仅预测;本例输入均为有限数。若连续 3 次拒绝,可把它作为示例告警条件,核对时间戳、米与毫米的单位、初始状态以及 、 是否过小。真实加速也可能超限,不能仅凭大分数判定传感器故障。
日志应保留全部有效观测的门控前 、 和接收标记。只统计已接收观测会主动删掉大分数,不能再据其均值偏小断言滤波器更准确。若采样间隔变化,还应按实际 重算 和 ,再判断是否超限。