自动控制 / 系统课时
用先验协方差表示初始不确定性
用先验协方差表示初始不确定性
同样给出位置 2 m、速度 1 m/s 的初始估计,可能来自可靠定位,也可能只是粗略猜测。本篇把这种可信程度写成先验协方差矩阵 ,读懂各元素的含义,并用 NumPy 检查取值是否合法。
学习目标
- 根据位置、速度的标准差构造 初始协方差矩阵。
- 解释非对角项的符号、单位及相关系数。
- 检查矩阵对称性与正半定性,识别不合理的初始化。
把估计值和误差尺度分开
状态按位置、速度排列为 ,初始估计为 ,两个分量分别使用 m 和 m/s。本文的 指接收新测量前、与这个初始估计配套的误差协方差;它的行列顺序必须与状态一致。
定义估计误差 。以下假设初始化没有系统偏差,即 。对角项记录各分量的误差方差,非对角项记录两种误差的交叉协方差。genui{"citation":{"refs":["turn2view0","turn1view0"]}}
| 误差分量 | 假设的标准差 | 应填入的对角项 |
|---|---|---|
| 位置 | ||
| 速度 |
对角项越小,表示对相应初始估计越确信。不能把标准差 2 和 0.5 直接填上去,也不能从估计值 2 和 1 推算出误差大小。标准差描述分散程度,不是误差上限。
非对角项记录误差的相关性
当两个标准差均大于零时,,其中相关系数满足 。取 ,得到 。按上述单位存储的矩阵数值为:genui{"citation":{"ref":"turn1view2"}}
正协方差表示位置误差与速度误差倾向同号,负协方差表示倾向异号,并不保证每次都如此。零协方差只表示不相关,一般不能直接推出独立;更不能因为“速度影响位置”,就随意指定一个正数。
下图假设误差联合高斯,、 分别表示位置和速度误差。各误差除以自身标准差后,坐标变为无量纲。两侧标准差完全相同,相关性改变了联合分布的方向;轮廓不表示误差只能落在内部。

用 NumPy 检查矩阵是否合法
协方差矩阵必须对称、正半定,即任何线性组合的误差方差都不能为负。对这个对称的 矩阵,只需再检查对角项非负且 。本例 ,满足条件。
在已安装 NumPy 的 Python 3.10 及以上环境中运行下面的完整脚本。代码保留原始 ,只把它标准化为无量纲矩阵 C0 来检查特征值;x0 使用一维数组,元素顺序仍为位置、速度。
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')预期输出:
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 = Truenp.linalg.eigvalsh 用于实对称矩阵,默认只读取下三角部分,因此必须先检查对称性。此处标准化矩阵的特征值都为正,原矩阵也正定;合法协方差允许零特征值,并非必须可逆。genui{"citation":{"ref":"turn2view1"}}
把 rho 改为 1.2 再运行,标准化特征值会变为 -0.2 和 2.2,正半定断言失败。这说明“对角项都是正数”还不够。代码中的 1e-12 仅容忍本例尺度下的浮点舍入误差。
让初始取值对应信息来源
如果位置定位较准,标准差为 ,但速度只是猜测,标准差为 ,就应填 、。没有相关性依据时可以暂设 ,但要把它当作待验证的建模假设。
验证时,可收集多次独立启动、同一初始化时刻相对于参考真值的误差,检查均值、方差与交叉协方差。不要直接对一条不断移动的位置轨迹求方差。如果误差长期偏向一侧,应先修正初始估计的偏差,单纯增大 不会消除偏差。
描述初始估计误差, 描述每步运动模型引入的过程噪声, 描述测量噪声。这里只有位置测量时, 是 ,而 是 ,两者不能照抄。genui{"citation":{"ref":"turn2view0"}}