自动控制 / 系统课时

测量丢帧后怎样维持状态预测

测量丢帧后怎样维持状态预测

传感器连续几帧没有读数时,滤波器仍要按时输出位置和速度。本篇用完整 NumPy 示例实现“每拍预测、有观测才更新”,并通过三帧缺测前后的方差变化,检查预测是否持续、输出是否正确反映不确定性。

学习目标

  • 实现每隔 1 s 预测一次、仅在有效位置读数到达时更新的循环。
  • 根据三帧缺测期间的位置与速度方差,判断不确定性是否正常传播。
  • 避免填零、重复使用旧读数和恢复观测时重复计算时间间隔。

丢帧只取消测量更新

用 x^=[p^,v^]T\hat{x}=[\hat{p},\hat{v}]^T 表示位置、速度估计,PP 表示估计误差协方差。没有已知控制输入时,每个采样时刻都先按运动模型预测;上标 −- 表示尚未融合本拍观测:

x^k−=Fx^k−1,Pk−=FPk−1FT+Q.\hat{x}_k^-=F\hat{x}_{k-1},\qquad P_k^-=FP_{k-1}F^T+Q.

有有效观测就执行测量更新;没有观测就令 x^k=x^k−\hat{x}_k=\hat{x}_k^-、Pk=Pk−P_k=P_k^-,把它们作为本拍结果及下一拍的起点。这一分支不计算新息,也不执行协方差收缩。genui{"citation":{"ref":"turn1view0"}}

例如当前估计为位置 2 m、速度 1 m/s,下一秒没有读数时仍应外推到 3 m。缺测不会让时间暂停,也不代表物体停下;因此应在预测之后判断观测是否可用。

每拍先预测,再根据观测是否有效选择更新或保留预测

用 NumPy 模拟连续三帧缺测

示例从 t=0t=0 的位置 0 m、速度 1 m/s 开始,初始标准差分别为 0.5 m、0.2 m/s,初始误差不相关。固定步长为 1 s,传感器只测位置,测量标准差为 0.5 m,因此 R=0.25 m2R=0.25\ \mathrm{m}^2。

过程噪声采用每个 1 s 区间内恒定、区间间独立的零均值随机加速度,标准差为 0.2 m/s20.2\ \mathrm{m/s^2}。令 G=[0.5(Δt)2,Δt]TG=[0.5(\Delta t)^2,\Delta t]^T,则 Q=0.22GGTQ=0.2^2GG^T,本例矩阵如下:

F=[1101],Q=[0.010.020.020.04].F=\begin{bmatrix}1&1\\0&1\end{bmatrix},\qquad Q=\begin{bmatrix}0.01&0.02\\0.02&0.04\end{bmatrix}.

观测矩阵 H=[1,0]H=[1,0] 只读取预测位置。有观测时,新息是 z−Hx^−z-H\hat{x}^-;代码中的增益 KK 把同一个新息分别用于修正位置和速度。这里只处理单个位置读数,所以新息方差 SS 是标量,可直接相除。

在已安装 NumPy 的 Python 3 环境中运行下面完整代码。固定读数依次对应 1~7 s,3~5 s 用 None 或 np.nan 表示缺测;不用随机数,便于逐行核对结果。此处的有效性检查只排除缺测与非有限值,有限的异常读数还需另行判定。

python
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}')

预期输出:

text
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。

位置方差增长不仅来自 QppQ_{pp},还因为速度误差会随时间积累成位置误差。位置标准差应取 Ppp\sqrt{P_{pp}},本例由约 0.36 m 增至 1.26 m;不能把方差 1.597 直接解释为“误差 1.597 m”。

从某个估计连续执行 nn 次预测,且 FF、QQ 保持不变,把预测公式展开可得:

Pk+n=FnPk(Fn)T+∑j=0n−1FjQ(Fj)T.P_{k+n}=F^nP_k(F^n)^T+\sum_{j=0}^{n-1}F^jQ(F^j)^T.

求和中的每一项都对应某一拍注入的过程噪声及其后续传播。把多拍预测合并时,不能只加一次原来的 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 时便应触发。预测无法发现缺测期间的突然加速,PP 也只反映所设模型与噪声假设;对外输出应保留“仅预测”状态。

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

目录与阅读设置

阅读设置

17

记录一个想法

测量丢帧后怎样维持状态预测

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

登录哪都通