自动控制 / 系统课时

用马氏距离排除离群测量

用马氏距离排除离群测量

位置传感器偶尔跳出一个尖峰,直接更新可能同时拉偏位置和速度。本篇用新息的马氏距离平方判断观测与预测是否一致,在更新前拒绝异常读数,并用同一组带噪测量验证轨迹误差和后续恢复情况。

学习目标

  • 用新息及其协方差计算马氏距离平方,为一维位置观测选择 99% 统计门限。
  • 实现接收与拒绝两条分支,拒绝时保留当前预测状态和预测协方差。
  • 对比包含两次离群测量的完整轨迹,检查拒绝位置、恢复更新和位置 RMSE。

把位置偏差换成无量纲分数

设状态为位置和速度 x=[p,v]Tx=[p,v]^T,传感器只测位置,因此 H=[1,0]H=[1,0]。记 x^k−\hat{x}_k^- 为当前时刻更新前的状态预测,Pk−P_k^- 为对应误差协方差,RR 为测量噪声协方差;标量位置观测中,RR 就是测量方差。

先计算新息,也就是实际读数减去预测读数;再把预测误差与测量噪声投影到同一个测量空间:

νk=zk−Hx^k−,Sk=HPk−HT+R\nu_k=z_k-H\hat{x}_k^-,\qquad S_k=HP_k^-H^T+R

用于门控的分数是马氏距离平方,也称归一化新息平方 NIS。验证门采用下面的二次型;计算依据是更新前的新息,而不是被当前观测修正后的残差。

dk2=νkTSk−1νkd_k^2=\nu_k^T S_k^{-1}\nu_k

一维时就是 dk2=νk2/Skd_k^2=\nu_k^2/S_k,没有单位。比如预测位置为 10 m,预测位置方差为 4 m²,测量方差为 1 m²,则 S=5S=5 m²。若读数为 18 m,新息为 8 m,得到 d2=64/5=12.8d^2=64/5=12.8。分母若只用 RR,就漏掉了预测自身的不确定性。

按测量维数选择统计门限

如果未筛选的新息服从零均值高斯分布,且其真实协方差就是 SS,白化后的平方和服从自由度为测量维数 mm 的卡方分布。取门限 γ=χm,0.992\gamma=\chi^2_{m,0.99},只在 d2≤γd^2\leq\gamma 时接收。

测量内容 测量维数 mm 95% 门限 99% 门限
一个位置读数 1 3.841 6.635
二维位置读数 2 5.991 9.210

表中数值来自 NIST 卡方分布临界值表。本例状态虽然有位置、速度两个分量,但观测只有一个分量,所以用 6.635。若代码计算的是距离 dd 而非平方 d2d^2,对应门限应为 6.635≈2.576\sqrt{6.635}\approx2.576。

前面的 12.8>6.63512.8>6.635,因此拒绝该观测。若同样偏差对应的 SS 增大到 20 m²,分数就降为 3.2,可以通过;门控考虑的是偏差相对于不确定性的大小,不能替换成固定米数的跳变判断。

拒绝观测时仍然推进预测

每拍先按运动模型得到 x^k−\hat{x}_k^- 和 Pk−P_k^-,再计算新息与门限分数。上标 ++ 表示本拍处理后的结果。接收后才计算增益并更新均值、协方差;拒绝时执行 x^k+=x^k−\hat{x}_k^+=\hat{x}_k^-、Pk+=Pk−P_k^+=P_k^-,下一拍继续从这里预测。

观测更新前进行马氏门控,拒绝分支保留当前先验并继续推进时间

不要把异常读数替换成预测位置再照常更新:这样虽然新息变成 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² 构造 Q=0.052ggTQ=0.05^2gg^T,其中 g=[Δt2/2,Δt]Tg=[\Delta t^2/2,\Delta t]^T,为未建模运动预留不确定性。此处真值本身没有加入过程扰动。

代码以 np.inf 关闭门控形成对照,两次运行共享同一测量数组。np.linalg.solve 求解线性方程,直接算出 S−1νS^{-1}\nu,不显式构造逆矩阵;这里 SS 为正定矩阵,满足该接口对可解方阵的要求。

python
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")

预期输出:

text
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 形式更新协方差,随后对称化以消除浮点舍入造成的小误差。固定为两维状态、一维观测时,处理 NN 个读数的时间为 O(N)O(N);示例保存全部轨迹也需 O(N)O(N) 空间,在线只保留当前状态与协方差可使用 O(1)O(1) 空间。

验收轨迹并定位连续拒绝

首先检查拒绝编号是否为 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 次拒绝,可把它作为示例告警条件,核对时间戳、米与毫米的单位、初始状态以及 QQ、RR 是否过小。真实加速也可能超限,不能仅凭大分数判定传感器故障。

日志应保留全部有效观测的门控前 d2d^2、SS 和接收标记。只统计已接收观测会主动删掉大分数,不能再据其均值偏小断言滤波器更准确。若采样间隔变化,还应按实际 Δt\Delta t 重算 FF 和 QQ,再判断是否超限。

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

目录与阅读设置

阅读设置

17

记录一个想法

用马氏距离排除离群测量

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

登录哪都通