自动控制 / 系统课时
初始协方差如何影响收敛速度
初始协方差如何影响收敛速度
初始位置偏了 5 m、速度也估错时,把协方差设得很小会让滤波器过分相信错误初值。本篇固定运动模型、噪声参数和测量记录,只改变初始协方差,比较首拍增益、启动误差与进入误差阈值的时间。
学习目标
- 根据位置、速度的初始标准差构造 。
- 用同一份位置测量比较两种 的增益和前 10 拍误差。
- 区分增益稳定、误差暂时达标与统计一致性。
把初始标准差换成协方差
状态按 排列,位置单位为 m,速度单位为 m/s。两组估计都从 的 开始,真实初值为 。只修改 ,不修改初始均值。
假设初始位置与速度误差不相关,则 。对角线上填的是方差,两个分量的单位分别为 m² 和 (m/s)²;非对角元设为 0 是实验假设。
| 设置 | 位置标准差 | 速度标准差 | 初始协方差 |
|---|---|---|---|
small:过强初始信心 |
0.1 m | 0.1 m/s | |
broad:较宽初始范围 |
5 m | 2 m/s |
把位置标准差从 0.1 m 改为 5 m,不会把估计位置从 5 m 自动移回真值;它改变的是后续更新对这个初值的信任程度。较宽的范围也不等于已经证明协方差校准正确。
沿第一拍追踪增益变化
采样间隔取 1 s,模型按 、 传播,观测矩阵 只读取位置。测量标准差为 1 m,因此 。
滤波器固定采用 ,其中 ,对应每个 1 s 区间内恒定、区间间独立的随机加速度模型,加速度标准差为 0.1 m/s²。本实验真值保持严格匀速, 仅作为模型容差,因此这里只比较启动表现。
先将初值预测到 ,再融合该时刻测量。预测协方差为:
对当前单位置观测,位置与速度增益分别为:
这里 是预测速度误差与位置误差的互协方差。两路增益乘同一个位置新息,再加回各自的预测分量; 无量纲, 的单位为 1/s。递推关系可对照 Welch 与 Bishop 的原文第 5 页。genui{"citation":{"ref":"turn3view0"}}
首拍 small 的 ,得到 ;broad 的 ,得到 。前者只用约 2.2% 的位置新息修正预测,后者用约 96.7%。

用同一组测量运行对照实验
下面程序可在已安装 NumPy 的 Python 环境直接运行,预期输出已在 NumPy 2.3.5 下核验。真值在第 秒为位置 m、速度 1 m/s。随机种子为 7,40 个位置读数只生成一次,两组滤波器共享。
误差定义为估计减真值。前 10 拍的位置、速度分别平方求平均再开根号,得到 RMSE。另记录首次连续 5 拍同时满足位置误差绝对值小于 1 m、速度误差绝对值小于 0.3 m/s 的窗口起点;不存在则输出 None。
import numpy as np
F = np.array([[1.0, 1.0], [0.0, 1.0]])
H = np.array([[1.0, 0.0]])
G = np.array([[0.5], [1.0]])
Q = 0.1**2 * (G @ G.T)
R = np.array([[1.0]])
I = np.eye(2)
# 同一真值与测量只生成一次,两组实验仅改变 P0。
t = np.arange(1.0, 41.0)
truth = np.column_stack((t, np.ones_like(t)))
z = truth[:, 0] + np.random.default_rng(7).normal(0.0, 1.0, len(t))
def run(P0):
x = np.array([5.0, 0.0])
P = P0.copy()
states, gains = [], []
for measurement in z:
# 从上一拍预测到当前测量时刻,再做更新。
x = F @ x
P = F @ P @ F.T + Q
innovation = np.array([measurement]) - H @ x
S = H @ P @ H.T + R
K = np.linalg.solve(S, H @ P).T
x = x + K @ innovation
# Joseph 形式同时保留两项不确定性,减少数值误差影响。
A = I - K @ H
P = A @ P @ A.T + K @ R @ K.T
states.append(x.copy())
gains.append(K[:, 0].copy())
return np.array(states), np.array(gains)
for name, P0 in [("small", np.diag([0.01, 0.01])),
("broad", np.diag([25.0, 4.0]))]:
states, gains = run(P0)
error = states - truth
rmse = np.sqrt(np.mean(error[:10]**2, axis=0))
# 报告首个连续 5 拍双指标合格窗口的起点,而非单次过线。
ok = (np.abs(error[:, 0]) < 1.0) & (np.abs(error[:, 1]) < 0.3)
start = next((i + 1 for i in range(len(ok) - 4)
if np.all(ok[i:i + 5])), None)
print(f"{name}: K1=({gains[0, 0]:.3f}, {gains[0, 1]:.3f}), "
f"RMSE10=({rmse[0]:.3f} m, {rmse[1]:.3f} m/s), start={start}")
print(f"{name}: K40=({gains[-1, 0]:.3f}, {gains[-1, 1]:.3f})")预期输出:
small: K1=(0.022, 0.015), RMSE10=(1.977 m, 0.995 m/s), start=11
small: K40=(0.360, 0.080)
broad: K1=(0.967, 0.133), RMSE10=(0.547 m, 0.554 m/s), start=7
broad: K40=(0.360, 0.080)K1、K40 括号内依次为位置增益和速度增益,RMSE10 依次为位置与速度的前 10 拍 RMSE。代码用 np.linalg.solve 求解增益,协方差采用 Joseph 更新形式,避免为了比较初值而引入两套更新算法。
读懂结果并设置实际初值
本次记录中,较宽初始范围把前 10 拍位置 RMSE 从 1.977 m 降到 0.547 m,速度 RMSE 从 0.995 m/s 降到 0.554 m/s。start=7 表示第 7~11 拍同时达标,在线运行到第 11 拍才能确认;较小协方差组的窗口从第 11 拍开始。
不要只看位置暂时接近真值:small 在第 4 拍的位置误差约为 −0.207 m,速度误差仍约为 −1.328 m/s。连续窗口也只是本实验的工程判据,不能保证此后永不越界。
首拍较宽协方差组的速度约为 −0.534 m/s,反而比另一组的 −0.059 m/s 离真值 1 m/s 更远。原因是负的位置新息通过正互协方差同时下调速度;后续位置观测才逐步分清初始位置偏差与速度偏差。
两组第 40 拍增益都约为 ,表明本例初始协方差的影响逐渐减弱,不能据此断言状态误差归零或置信区间已达到标称覆盖率。
实际初始化时,若位置来自标准差 1 m 的首次读数,速度只是猜为 0 且初始标准差估为 2 m/s,在误差不相关假设下可用 。首次读数已用于初始化时,不应再把它当作独立新测量重复融合。
应让 反映初始信息的可信程度。初值确实准确时,盲目放大它会增强测量噪声对启动估计的影响;运行中持续偏差则要检查模型、时间对齐和 ,不要每拍重设 来掩盖问题。