自动控制 / 系统课时
采样间隔变化时如何更新模型
采样间隔变化时如何更新模型
传感器标称每0.1秒采样一次,实际记录却可能相隔0.2或0.4秒。固定模型会把漏算的时间误当成运动变化。本篇按实际采样时间戳重算转移矩阵与过程噪声,并用同一组位置读数验证修改前后的差异。
学习目标
- 从测量采集时刻与当前状态时刻计算实际间隔 。
- 在同一噪声假设下同步重算 和 。
- 运行非均匀采样滤波,并用数值输出与分段预测检验实现。
让预测覆盖真实经过的时间
状态按位置、速度排列为 ,单位分别为米和米每秒。若0.3秒时估计位置为0.6米、速度为2米每秒,下一条测量采于0.7秒,就应传播0.4秒,得到预测位置1.4米;误用0.1秒只得到0.8米,少算0.6米。
记录当前状态所对应的时刻 ,收到采集时刻为 的测量后,计算 。时间戳必须来自同一时间基准,并统一成秒;例如400毫秒应换成0.4秒。网络接收时刻不能替代采集时刻,否则传输抖动也会被算进运动。
同时重算 F 和 Q
本例没有已知控制输入,采用零均值的连续白噪声加速度模型:随机扰动持续作用于速度,并与初始误差及测量噪声独立。其强度 是速度增量方差的增长率,单位为 ,不是一次加速度采样的方差。
上述公式来自连续白噪声模型的精确离散化;时间间隔变化时,在噪声强度保持不变的假设下保留同一个 ,重新计算矩阵各项。genui{"citation":{"ref":"turn1view0"}}
取 ,不同间隔得到下面的过程噪声分量。表中的 、、 分别表示本段新增的位置方差、位置与速度的互协方差、速度方差。
| (s) | (m²) | (m²/s) | (m²/s²) |
|---|---|---|---|
| 0.1 | 0.0002 | 0.003 | 0.06 |
| 0.4 | 0.0128 | 0.048 | 0.24 |
间隔扩大4倍,三个分量分别扩大64、16、4倍。因此,不能把旧 整体乘4,也不能只改 。实际预测还要传播已有协方差:,; 只是本段新增的不确定性。
按时间戳执行完整滤波
每条记录依次执行:计算实际间隔、重算 与 、传播均值与协方差、用该时刻读数更新、推进状态时刻。新息是测量位置减预测位置;本例用 提取位置,按 和 计算修正权重。

在 Python 3.10+、已安装 NumPy 的环境运行下列完整程序。设定真实运动为 、,四个位置读数加入固定误差以便复现;初始估计为 ,位置和速度的初始标准差均为0.5,对应各自单位。
测量标准差设为0.2米,因此 ;过程噪声强度取 。函数默认按实际间隔运行,第二次调用故意固定为0.1秒,作为错误对照。时间与位置记录都是教学数据,不依赖系统时钟或随机数。
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")预期输出:
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。较大的速度误差说明时间尺度错误会传入隐藏状态。
程序末尾验证另一个与测量值无关的条件:同一个恒定 下,中间没有观测更新时,先预测0.1秒再预测0.2秒,应与直接预测0.3秒一致。这里 对应第一段, 对应第二段:
第一段噪声还会在第二段继续传播,因此不能简单写成 。两条断言通过后才打印成功标记。状态维数固定为2时,每步计算与在线状态存储均为 ;处理 条记录为 ,本例额外保存输出记录,使用 内存。
遇到异常时间戳时怎么处理
- 零间隔或重复包: 正常模式要求时间戳有限且严格递增,遇到非正间隔会报错。同刻的独立观测可以用 、 跳过预测后更新;相同数据包应去重,不能重复融合。
- 乱序测量: 当 时,先缓冲排序,或明确丢弃过时数据;需要保留迟到观测时应采用回滚重放等延迟处理。不要对间隔取绝对值。
- 缺测或已经预测过: 缺测期间只预测。如果状态已经从0.3秒预测到0.5秒,随后收到0.7秒的测量,剩余间隔应为0.2秒,不能再次使用0.4秒。
- 长时间中断: 实际间隔为2秒,就不能悄悄截成0.1秒。若运动模型在该间隔内仍适用,使用完整间隔;超过业务验证的模型适用范围时,标记估计失效或重新初始化。
对于精度不变的单次位置读数, 和 不必随间隔缩放;如果读数改为不同积分时长的平均值,则要重新评估测量噪声。接入真实数据后,若日志始终显示0.1秒而采集时间戳存在0.4秒间隔,应先修复时间差传递,再调整 。