自动控制 / 系统课时

初始协方差如何影响收敛速度

初始协方差如何影响收敛速度

初始位置偏了 5 m、速度也估错时,把协方差设得很小会让滤波器过分相信错误初值。本篇固定运动模型、噪声参数和测量记录,只改变初始协方差,比较首拍增益、启动误差与进入误差阈值的时间。

学习目标

  • 根据位置、速度的初始标准差构造 P0P_0。
  • 用同一份位置测量比较两种 P0P_0 的增益和前 10 拍误差。
  • 区分增益稳定、误差暂时达标与统计一致性。

把初始标准差换成协方差

状态按 x=[p,v]Tx=[p,v]^T 排列,位置单位为 m,速度单位为 m/s。两组估计都从 t=0t=0 的 x^0=[5,0]T\hat x_0=[5,0]^T 开始,真实初值为 x0=[0,1]Tx_0=[0,1]^T。只修改 P0P_0,不修改初始均值。

假设初始位置与速度误差不相关,则 P0=diag⁡(σp2,σv2)P_0=\operatorname{diag}(\sigma_p^2,\sigma_v^2)。对角线上填的是方差,两个分量的单位分别为 m² 和 (m/s)²;非对角元设为 0 是实验假设。

设置 位置标准差 速度标准差 初始协方差
small:过强初始信心 0.1 m 0.1 m/s diag⁡(0.01,0.01)\operatorname{diag}(0.01,0.01)
broad:较宽初始范围 5 m 2 m/s diag⁡(25,4)\operatorname{diag}(25,4)

把位置标准差从 0.1 m 改为 5 m,不会把估计位置从 5 m 自动移回真值;它改变的是后续更新对这个初值的信任程度。较宽的范围也不等于已经证明协方差校准正确。

沿第一拍追踪增益变化

采样间隔取 1 s,模型按 pk+1=pk+vkΔtp_{k+1}=p_k+v_k\Delta t、vk+1=vkv_{k+1}=v_k 传播,观测矩阵 H=[1,0]H=[1,0] 只读取位置。测量标准差为 1 m,因此 R=1 m2R=1\ \mathrm{m}^2。

滤波器固定采用 Q=0.01GGTQ=0.01GG^T,其中 G=[0.5,1]TG=[0.5,1]^T,对应每个 1 s 区间内恒定、区间间独立的随机加速度模型,加速度标准差为 0.1 m/s²。本实验真值保持严格匀速,QQ 仅作为模型容差,因此这里只比较启动表现。

先将初值预测到 t=1t=1,再融合该时刻测量。预测协方差为:

P1−=FP0FT+QP_1^-=FP_0F^T+Q

对当前单位置观测,位置与速度增益分别为:

Kp,1=Ppp,1−Ppp,1−+R,Kv,1=Pvp,1−Ppp,1−+RK_{p,1}=\frac{P_{pp,1}^-}{P_{pp,1}^-+R},\qquad K_{v,1}=\frac{P_{vp,1}^-}{P_{pp,1}^-+R}

这里 Pvp,1−P_{vp,1}^- 是预测速度误差与位置误差的互协方差。两路增益乘同一个位置新息,再加回各自的预测分量;KpK_p 无量纲,KvK_v 的单位为 1/s。递推关系可对照 Welch 与 Bishop 的原文第 5 页。genui{"citation":{"ref":"turn3view0"}}

首拍 small 的 Ppp,1−=0.0225P_{pp,1}^-=0.0225,得到 Kp,1≈0.022K_{p,1}\approx0.022;broad 的 Ppp,1−=29.0025P_{pp,1}^-=29.0025,得到 Kp,1≈0.967K_{p,1}\approx0.967。前者只用约 2.2% 的位置新息修正预测,后者用约 96.7%。

不同初始协方差经预测改变首拍增益,同一位置新息产生不同的状态修正

用同一组测量运行对照实验

下面程序可在已安装 NumPy 的 Python 环境直接运行,预期输出已在 NumPy 2.3.5 下核验。真值在第 kk 秒为位置 kk m、速度 1 m/s。随机种子为 7,40 个位置读数只生成一次,两组滤波器共享。

误差定义为估计减真值。前 10 拍的位置、速度分别平方求平均再开根号,得到 RMSE。另记录首次连续 5 拍同时满足位置误差绝对值小于 1 m、速度误差绝对值小于 0.3 m/s 的窗口起点;不存在则输出 None。

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

预期输出:

text
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 拍增益都约为 (0.360,0.080)T(0.360,0.080)^T,表明本例初始协方差的影响逐渐减弱,不能据此断言状态误差归零或置信区间已达到标称覆盖率。

实际初始化时,若位置来自标准差 1 m 的首次读数,速度只是猜为 0 且初始标准差估为 2 m/s,在误差不相关假设下可用 P0=diag⁡(1,4)P_0=\operatorname{diag}(1,4)。首次读数已用于初始化时,不应再把它当作独立新测量重复融合。

应让 P0P_0 反映初始信息的可信程度。初值确实准确时,盲目放大它会增强测量噪声对启动估计的影响;运行中持续偏差则要检查模型、时间对齐和 Q,RQ,R,不要每拍重设 P0P_0 来掩盖问题。

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

目录与阅读设置

阅读设置

17

记录一个想法

初始协方差如何影响收敛速度

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

登录哪都通