自动控制 / 系统课时

采样间隔变化时如何更新模型

采样间隔变化时如何更新模型

传感器标称每0.1秒采样一次,实际记录却可能相隔0.2或0.4秒。固定模型会把漏算的时间误当成运动变化。本篇按实际采样时间戳重算转移矩阵与过程噪声,并用同一组位置读数验证修改前后的差异。

学习目标

  • 从测量采集时刻与当前状态时刻计算实际间隔 Δt\Delta t。
  • 在同一噪声假设下同步重算 F(Δt)F(\Delta t) 和 Q(Δt)Q(\Delta t)。
  • 运行非均匀采样滤波,并用数值输出与分段预测检验实现。

让预测覆盖真实经过的时间

状态按位置、速度排列为 x=[p,v]Tx=[p,v]^\mathsf{T},单位分别为米和米每秒。若0.3秒时估计位置为0.6米、速度为2米每秒,下一条测量采于0.7秒,就应传播0.4秒,得到预测位置1.4米;误用0.1秒只得到0.8米,少算0.6米。

记录当前状态所对应的时刻 tstatet_{\mathrm{state}},收到采集时刻为 tkt_k 的测量后,计算 Δtk=tk−tstate\Delta t_k=t_k-t_{\mathrm{state}}。时间戳必须来自同一时间基准,并统一成秒;例如400毫秒应换成0.4秒。网络接收时刻不能替代采集时刻,否则传输抖动也会被算进运动。

同时重算 F 和 Q

本例没有已知控制输入,采用零均值的连续白噪声加速度模型:随机扰动持续作用于速度,并与初始误差及测量噪声独立。其强度 qq 是速度增量方差的增长率,单位为 m2/s3\mathrm{m^2/s^3},不是一次加速度采样的方差。

F(Δt)=[1Δt01],Q(Δt)=q[Δt3/3Δt2/2Δt2/2Δt]F(\Delta t)=\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix},\qquad Q(\Delta t)=q\begin{bmatrix}\Delta t^3/3&\Delta t^2/2\\\Delta t^2/2&\Delta t\end{bmatrix}

上述公式来自连续白噪声模型的精确离散化;时间间隔变化时,在噪声强度保持不变的假设下保留同一个 qq,重新计算矩阵各项。genui{"citation":{"ref":"turn1view0"}}

取 q=0.6q=0.6,不同间隔得到下面的过程噪声分量。表中的 QppQ_{pp}、QpvQ_{pv}、QvvQ_{vv} 分别表示本段新增的位置方差、位置与速度的互协方差、速度方差。

Δt\Delta t(s) QppQ_{pp}(m²) QpvQ_{pv}(m²/s) QvvQ_{vv}(m²/s²)
0.1 0.0002 0.003 0.06
0.4 0.0128 0.048 0.24

间隔扩大4倍,三个分量分别扩大64、16、4倍。因此,不能把旧 QQ 整体乘4,也不能只改 FF。实际预测还要传播已有协方差:x^−=Fx^\hat{x}^{-}=F\hat{x},P−=FPFT+QP^{-}=FPF^\mathsf{T}+Q;QQ 只是本段新增的不确定性。

按时间戳执行完整滤波

每条记录依次执行:计算实际间隔、重算 FF 与 QQ、传播均值与协方差、用该时刻读数更新、推进状态时刻。新息是测量位置减预测位置;本例用 H=[1,0]H=[1,0] 提取位置,按 S=HP−HT+RS=HP^{-}H^\mathsf{T}+R 和 K=P−HT/SK=P^{-}H^\mathsf{T}/S 计算修正权重。

实际时间差同时决定状态传播与过程噪声,预测完成后再融合当前测量

在 Python 3.10+、已安装 NumPy 的环境运行下列完整程序。设定真实运动为 p=2tp=2t、v=2v=2,四个位置读数加入固定误差以便复现;初始估计为 [0,2]T[0,2]^\mathsf{T},位置和速度的初始标准差均为0.5,对应各自单位。

测量标准差设为0.2米,因此 R=0.04R=0.04;过程噪声强度取 q=0.6q=0.6。函数默认按实际间隔运行,第二次调用故意固定为0.1秒,作为错误对照。时间与位置记录都是教学数据,不依赖系统时钟或随机数。

python
import numpy as np

def make_model(dt, q):
    # q 是速度增量方差增长率,不是单次加速度方差。
    if not np.isfinite(dt) or dt <= 0:
        raise ValueError("dt must be finite and positive")
    if not np.isfinite(q) or q < 0:
        raise ValueError("q must be finite and nonnegative")
    F = np.array([[1.0, dt], [0.0, 1.0]])
    Q = q * np.array([[dt**3 / 3, dt**2 / 2],
                      [dt**2 / 2, dt]])
    return F, Q

def run_filter(fixed_dt=None):
    # 每个位置读数与其采集时刻绑定,时间统一为秒。
    records = [(0.1, 0.24), (0.3, 0.55),
               (0.7, 1.50), (1.0, 1.92)]
    x = np.array([0.0, 2.0])
    P = np.diag([0.25, 0.25])
    H = np.array([[1.0, 0.0]])
    R = np.array([[0.04]])
    I = np.eye(2)
    t_state = 0.0
    rows = []
    for t, z in records:
        dt = t - t_state
        # fixed_dt 只用于构造错误对照;正常运行使用实际 dt。
        F, Q = make_model(dt if fixed_dt is None else fixed_dt, 0.6)
        x = F @ x
        P = F @ P @ F.T + Q
        p_prior = x[0]

        # 先预测到测量时刻,再用这一次位置读数更新。
        innovation = z - (H @ x).item()
        S = (H @ P @ H.T + R).item()
        K = P @ H.T / S
        x = x + K[:, 0] * innovation
        # Joseph 形式有助于保持协方差的半正定性。
        A = I - K @ H
        P = A @ P @ A.T + K @ R @ K.T
        P = (P + P.T) / 2
        t_state = t
        rows.append([t, dt, p_prior, x[0], x[1]])
    return np.array(rows)

actual = run_filter()
frozen = run_filter(fixed_dt=0.1)
print("t dt p_prior p_post v_post")
for row in actual:
    print(" ".join(f"{value:.3f}" for value in row))
print(f"actual_dt final: p={actual[-1, 3]:.3f}, v={actual[-1, 4]:.3f}")
print(f"fixed_dt final: p={frozen[-1, 3]:.3f}, v={frozen[-1, 4]:.3f}")

# q 恒定且无中途观测时,分段预测应等价于一次预测。
F1, Q1 = make_model(0.1, 0.6)
F2, Q2 = make_model(0.2, 0.6)
F3, Q3 = make_model(0.3, 0.6)
assert np.allclose(F3, F2 @ F1, rtol=1e-12, atol=1e-12)
assert np.allclose(Q3, F2 @ Q1 @ F2.T + Q2, rtol=1e-12, atol=1e-12)
print("split_check=True")

预期输出:

text
t dt p_prior p_post v_post
0.100 0.100 0.200 0.235 2.004
0.300 0.200 0.635 0.588 1.931
0.700 0.400 1.360 1.465 2.128
1.000 0.300 2.104 1.975 1.900
actual_dt final: p=1.975, v=1.900
fixed_dt final: p=1.546, v=3.424
split_check=True

用输出与分段预测验收

先检查第二列是否为0.1、0.2、0.4、0.3秒,再检查预测发生在测量更新之前。输出位置单位为米、速度单位为米每秒:最后时刻真值为位置2、速度2;实际间隔得到1.975和1.900,固定间隔得到1.546和3.424。较大的速度误差说明时间尺度错误会传入隐藏状态。

程序末尾验证另一个与测量值无关的条件:同一个恒定 qq 下,中间没有观测更新时,先预测0.1秒再预测0.2秒,应与直接预测0.3秒一致。这里 F1,Q1F_1,Q_1 对应第一段,F2,Q2F_2,Q_2 对应第二段:

F(0.3)=F2F1,Q(0.3)=F2Q1F2T+Q2F(0.3)=F_2F_1,\qquad Q(0.3)=F_2Q_1F_2^\mathsf{T}+Q_2

第一段噪声还会在第二段继续传播,因此不能简单写成 Q1+Q2Q_1+Q_2。两条断言通过后才打印成功标记。状态维数固定为2时,每步计算与在线状态存储均为 O(1)O(1);处理 NN 条记录为 O(N)O(N),本例额外保存输出记录,使用 O(N)O(N) 内存。

遇到异常时间戳时怎么处理

  • 零间隔或重复包: 正常模式要求时间戳有限且严格递增,遇到非正间隔会报错。同刻的独立观测可以用 F(0)=IF(0)=I、Q(0)=0Q(0)=0 跳过预测后更新;相同数据包应去重,不能重复融合。
  • 乱序测量: 当 Δt<0\Delta t<0 时,先缓冲排序,或明确丢弃过时数据;需要保留迟到观测时应采用回滚重放等延迟处理。不要对间隔取绝对值。
  • 缺测或已经预测过: 缺测期间只预测。如果状态已经从0.3秒预测到0.5秒,随后收到0.7秒的测量,剩余间隔应为0.2秒,不能再次使用0.4秒。
  • 长时间中断: 实际间隔为2秒,就不能悄悄截成0.1秒。若运动模型在该间隔内仍适用,使用完整间隔;超过业务验证的模型适用范围时,标记估计失效或重新初始化。

对于精度不变的单次位置读数,HH 和 RR 不必随间隔缩放;如果读数改为不同积分时长的平均值,则要重新评估测量噪声。接入真实数据后,若日志始终显示0.1秒而采集时间戳存在0.4秒间隔,应先修复时间差传递,再调整 qq。

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

目录与阅读设置

阅读设置

17

记录一个想法

采样间隔变化时如何更新模型

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

登录哪都通