自动控制 / 系统课时
测量丢帧后怎样维持状态预测
测量丢帧后怎样维持状态预测
传感器连续几帧没有读数时,滤波器仍要按时输出位置和速度。本篇用完整 NumPy 示例实现“每拍预测、有观测才更新”,并通过三帧缺测前后的方差变化,检查预测是否持续、输出是否正确反映不确定性。
学习目标
- 实现每隔 1 s 预测一次、仅在有效位置读数到达时更新的循环。
- 根据三帧缺测期间的位置与速度方差,判断不确定性是否正常传播。
- 避免填零、重复使用旧读数和恢复观测时重复计算时间间隔。
丢帧只取消测量更新
用 表示位置、速度估计, 表示估计误差协方差。没有已知控制输入时,每个采样时刻都先按运动模型预测;上标 表示尚未融合本拍观测:
有有效观测就执行测量更新;没有观测就令 、,把它们作为本拍结果及下一拍的起点。这一分支不计算新息,也不执行协方差收缩。genui{"citation":{"ref":"turn1view0"}}
例如当前估计为位置 2 m、速度 1 m/s,下一秒没有读数时仍应外推到 3 m。缺测不会让时间暂停,也不代表物体停下;因此应在预测之后判断观测是否可用。

用 NumPy 模拟连续三帧缺测
示例从 的位置 0 m、速度 1 m/s 开始,初始标准差分别为 0.5 m、0.2 m/s,初始误差不相关。固定步长为 1 s,传感器只测位置,测量标准差为 0.5 m,因此 。
过程噪声采用每个 1 s 区间内恒定、区间间独立的零均值随机加速度,标准差为 。令 ,则 ,本例矩阵如下:
观测矩阵 只读取预测位置。有观测时,新息是 ;代码中的增益 把同一个新息分别用于修正位置和速度。这里只处理单个位置读数,所以新息方差 是标量,可直接相除。
在已安装 NumPy 的 Python 3 环境中运行下面完整代码。固定读数依次对应 1~7 s,3~5 s 用 None 或 np.nan 表示缺测;不用随机数,便于逐行核对结果。此处的有效性检查只排除缺测与非有限值,有限的异常读数还需另行判定。
import numpy as np
# 状态时刻从 t=0 开始;每一项观测对应下一秒。
dt = 1.0
F = np.array([[1.0, dt], [0.0, 1.0]])
G = np.array([0.5 * dt**2, dt])
Q = 0.2**2 * np.outer(G, G)
H = np.array([[1.0, 0.0]])
R = 0.5**2
I = np.eye(2)
x = np.array([0.0, 1.0])
P = np.diag([0.25, 0.04])
measurements = [1.1, 1.9, None, np.nan, None, 6.2, 7.1]
print('t mode p_hat v_hat Ppp Pvv')
for t, z in enumerate(measurements, start=1):
# 无论是否收到观测,先把均值和协方差推进一拍。
x_pred = F @ x
P_pred = F @ P @ F.T + Q
if z is None or not np.isfinite(z):
x, P = x_pred, P_pred
mode = 'predict'
else:
innovation = z - (H @ x_pred).item()
S = (H @ P_pred @ H.T).item() + R
K = (P_pred @ H.T) / S
x = x_pred + K[:, 0] * innovation
# Joseph 形式保留测量噪声项,减轻数值舍入的影响。
A = I - K @ H
P = A @ P_pred @ A.T + R * (K @ K.T)
mode = 'update'
P = 0.5 * (P + P.T)
print(f'{t} {mode} {x[0]:.3f} {x[1]:.3f} '
f'{P[0, 0]:.3f} {P[1, 1]:.3f}')预期输出:
t mode p_hat v_hat Ppp Pvv
1 update 1.055 1.011 0.136 0.073
2 update 1.979 0.973 0.131 0.086
3 predict 2.952 0.973 0.342 0.126
4 predict 3.925 0.973 0.804 0.166
5 predict 4.897 0.973 1.597 0.206
6 update 6.173 1.051 0.230 0.076
7 update 7.145 1.023 0.159 0.081输出中的 predict 表示仅预测,update 表示预测后还融合了观测。p_hat、v_hat 的单位分别为 m、m/s;Ppp、Pvv 是位置和速度方差,单位分别为 m²、(m/s)²。
对照方差验证缺测影响
先检查 3~5 s:位置继续前移,速度估计保持约 0.973 m/s。这是匀速模型的外推结果,不代表真实速度已被观测确认。位置方差从 2 s 的约 0.131 增至 5 s 的约 1.597,速度方差每拍增加 0.04。
位置方差增长不仅来自 ,还因为速度误差会随时间积累成位置误差。位置标准差应取 ,本例由约 0.36 m 增至 1.26 m;不能把方差 1.597 直接解释为“误差 1.597 m”。
从某个估计连续执行 次预测,且 、 保持不变,把预测公式展开可得:
求和中的每一项都对应某一拍注入的过程噪声及其后续传播。把多拍预测合并时,不能只加一次原来的 Q。本例缺测段的两项方差都增长;一般模型下,不能据此断言协方差矩阵的所有元素都逐拍增大。
6 s 恢复测量后,位置方差降至约 0.230,说明新观测重新约束了状态。它没有归零,因为测量自身也带噪声。验收时应同时看到:缺测段仍有输出、方差按模型传播、恢复段重新执行更新。
恢复观测时保持时间轴一致
本例由固定时钟驱动,3、4、5 s 已各预测一次,处理 6 s 的读数时只需从 5 s 再推进 1 s。若再次按“距离上次观测已有 4 s”预测,就会把同一段时间重复计算。
若程序只在收到数据时运行,从 2 s 直接收到 6 s 的帧,应先补做四次 1 s 预测,再融合 6 s 的读数。也可以用等价的长步长模型,但必须同时计算对应的转移矩阵和累计过程噪声。
本例的随机加速度每秒独立重取,不能直接把总时长 4 s 代入单步 G 后仍沿用同一加速度方差;那会变成整个 4 s 内共用一个随机加速度的另一种模型。
每次输出都记录估计对应的时间戳与最近一次有效观测时间。已推进到 6 s 后才到达的 4 s 读数属于迟到数据,不能当作 6 s 观测直接更新;应走单独的历史回放或丢弃策略。
避免把空缺伪装成新数据
| 错误做法 | 造成的结果 | 本例应采用的做法 |
|---|---|---|
| 缺测时填入位置 0 | 把不存在的零位置读数用于校正 | 保留缺测标记,跳过更新 |
| 每拍重复上一次读数 | 把过期数据当作新的独立信息 | 按测量时间戳去重 |
| 把预测位置当作测量值 | 新息虽为零,协方差仍会无依据地收缩 | 直接保留预测均值和协方差 |
| 只推进均值、冻结协方差 | 输出继续移动,却虚报可信度 | 同时计算 F @ P @ F.T + Q |
0.0 可以是真实位置,不能用 if z: 判断是否缺测。np.nan 要在计算新息之前拦截,否则一次减法就可能把后续状态全部污染为非有限值。
长时间缺测时,可按应用容许误差设置降级条件。例如演示中规定位置标准差超过 1 m 就标记为低可信,5 s 时便应触发。预测无法发现缺测期间的突然加速, 也只反映所设模型与噪声假设;对外输出应保留“仅预测”状态。