自动控制 / 系统课时

用先验协方差表示初始不确定性

用先验协方差表示初始不确定性

同样给出位置 2 m、速度 1 m/s 的初始估计,可能来自可靠定位,也可能只是粗略猜测。本篇把这种可信程度写成先验协方差矩阵 P0P_0,读懂各元素的含义,并用 NumPy 检查取值是否合法。

学习目标

  • 根据位置、速度的标准差构造 2×22\times2 初始协方差矩阵。
  • 解释非对角项的符号、单位及相关系数。
  • 检查矩阵对称性与正半定性,识别不合理的初始化。

把估计值和误差尺度分开

状态按位置、速度排列为 x0=[p0,v0]Tx_0=[p_0,v_0]^T,初始估计为 x^0=[2,1]T\hat{x}_0=[2,1]^T,两个分量分别使用 m 和 m/s。本文的 P0P_0 指接收新测量前、与这个初始估计配套的误差协方差;它的行列顺序必须与状态一致。

定义估计误差 e0=x0−x^0e_0=x_0-\hat{x}_0。以下假设初始化没有系统偏差,即 E[e0]=0E[e_0]=0。对角项记录各分量的误差方差,非对角项记录两种误差的交叉协方差。genui{"citation":{"refs":["turn2view0","turn1view0"]}}

P0=E[e0e0T]=[σp2cpvcpvσv2]P_0=E[e_0e_0^T]= \begin{bmatrix} \sigma_p^2 & c_{pv}\\ c_{pv} & \sigma_v^2 \end{bmatrix}
误差分量 假设的标准差 应填入的对角项
位置 2 m2\,\mathrm{m} Ppp=4 m2P_{pp}=4\,\mathrm{m}^2
速度 0.5 m/s0.5\,\mathrm{m/s} Pvv=0.25 (m/s)2P_{vv}=0.25\,(\mathrm{m/s})^2

对角项越小,表示对相应初始估计越确信。不能把标准差 2 和 0.5 直接填上去,也不能从估计值 2 和 1 推算出误差大小。标准差描述分散程度,不是误差上限。

非对角项记录误差的相关性

当两个标准差均大于零时,cpv=ρσpσvc_{pv}=\rho\sigma_p\sigma_v,其中相关系数满足 −1≤ρ≤1-1\leq\rho\leq1。取 ρ=0.6\rho=0.6,得到 cpv=0.6×2×0.5=0.6 m2/sc_{pv}=0.6\times2\times0.5=0.6\,\mathrm{m}^2/\mathrm{s}。按上述单位存储的矩阵数值为:genui{"citation":{"ref":"turn1view2"}}

P0=[40.60.60.25]P_0= \begin{bmatrix} 4 & 0.6\\ 0.6 & 0.25 \end{bmatrix}

正协方差表示位置误差与速度误差倾向同号,负协方差表示倾向异号,并不保证每次都如此。零协方差只表示不相关,一般不能直接推出独立;更不能因为“速度影响位置”,就随意指定一个正数。

下图假设误差联合高斯,epe_p、eve_v 分别表示位置和速度误差。各误差除以自身标准差后,坐标变为无量纲。两侧标准差完全相同,相关性改变了联合分布的方向;轮廓不表示误差只能落在内部。

相同标准差下,不相关与正相关初始误差的分布形状对比

用 NumPy 检查矩阵是否合法

协方差矩阵必须对称、正半定,即任何线性组合的误差方差都不能为负。对这个对称的 2×22\times2 矩阵,只需再检查对角项非负且 cpv2≤PppPvvc_{pv}^2\leq P_{pp}P_{vv}。本例 0.62=0.36≤4×0.25=10.6^2=0.36\leq4\times0.25=1,满足条件。

在已安装 NumPy 的 Python 3.10 及以上环境中运行下面的完整脚本。代码保留原始 P0P_0,只把它标准化为无量纲矩阵 C0 来检查特征值;x0 使用一维数组,元素顺序仍为位置、速度。

python
import numpy as np

# 固定分量顺序为位置、速度;sigma 中存标准差而不是方差。
x0 = np.array([2.0, 1.0])
sigma = np.array([2.0, 0.5])
rho = 0.6
assert (sigma > 0).all()
P0 = np.outer(sigma, sigma) * np.array([[1.0, rho], [rho, 1.0]])

# 先检查有限性和对称性,再进行特征值检查。
assert P0.shape == (x0.size, x0.size)
assert np.isfinite(P0).all()
assert np.allclose(P0, P0.T, rtol=0.0, atol=1e-12)

# 标准化后检查正半定性;此写法要求两个标准差都大于零。
C0 = P0 / np.outer(sigma, sigma)
eigenvalues = np.linalg.eigvalsh(C0)
assert eigenvalues.min() >= -1e-12
std = np.sqrt(np.diag(P0))
corr = P0[0, 1] / (std[0] * std[1])

print('x0 =', x0.tolist())
print('P0 =', P0.round(2).tolist())
print('std =', std.round(2).tolist())
print(f'rho = {corr:.2f}')
print('normalized eigenvalues =', eigenvalues.round(2).tolist())
print('valid = True')

预期输出:

text
x0 = [2.0, 1.0]
P0 = [[4.0, 0.6], [0.6, 0.25]]
std = [2.0, 0.5]
rho = 0.60
normalized eigenvalues = [0.4, 1.6]
valid = True

np.linalg.eigvalsh 用于实对称矩阵,默认只读取下三角部分,因此必须先检查对称性。此处标准化矩阵的特征值都为正,原矩阵也正定;合法协方差允许零特征值,并非必须可逆。genui{"citation":{"ref":"turn2view1"}}

把 rho 改为 1.2 再运行,标准化特征值会变为 -0.2 和 2.2,正半定断言失败。这说明“对角项都是正数”还不够。代码中的 1e-12 仅容忍本例尺度下的浮点舍入误差。

让初始取值对应信息来源

如果位置定位较准,标准差为 0.2 m0.2\,\mathrm{m},但速度只是猜测,标准差为 3 m/s3\,\mathrm{m/s},就应填 Ppp=0.04 m2P_{pp}=0.04\,\mathrm{m}^2、Pvv=9 (m/s)2P_{vv}=9\,(\mathrm{m/s})^2。没有相关性依据时可以暂设 cpv=0c_{pv}=0,但要把它当作待验证的建模假设。

验证时,可收集多次独立启动、同一初始化时刻相对于参考真值的误差,检查均值、方差与交叉协方差。不要直接对一条不断移动的位置轨迹求方差。如果误差长期偏向一侧,应先修正初始估计的偏差,单纯增大 P0P_0 不会消除偏差。

P0P_0 描述初始估计误差,QQ 描述每步运动模型引入的过程噪声,RR 描述测量噪声。这里只有位置测量时,RR 是 1×11\times1,而 P0P_0 是 2×22\times2,两者不能照抄。genui{"citation":{"ref":"turn2view0"}}

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

目录与阅读设置

阅读设置

17

记录一个想法

用先验协方差表示初始不确定性

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

登录哪都通