自动控制 / 系统课时
初始化状态估计和协方差矩阵
初始化状态估计和协方差矩阵
滤波循环开始前,初始状态、误差尺度和模型矩阵必须使用同一套变量顺序。本篇用一维位置与速度估计构造完整的 NumPy 初始化代码,并检查形状、浮点类型和协方差,得到可以接入后续预测与更新的起点。
学习目标
- 建立位置、速度顺序固定的列向量,匹配六个核心数组的形状。
- 将初始误差与噪声的标准差转换为协方差,区分 P、Q、R。
- 运行数值检查,识别广播错误、非法协方差和首测量重复使用。
先固定状态顺序和初始时刻
本例用 表示状态估计,位置单位为米,速度单位为米每秒。采样间隔为 0.1 秒,初值对应 ;第一条待处理测量位于 秒,因此正式循环先预测,再融合该时刻的读数。
所有数组使用 np.float64。状态和测量统一保留二维形状;矩阵相乘使用 @,* 对数组执行逐元素乘法。genui{"citation":{"ref":"turn2view0"}}
| 数组 | 形状 | 本例中的职责 |
|---|---|---|
x |
(2, 1) |
第一行位置,第二行速度 |
P |
(2, 2) |
初始状态估计误差协方差 |
F |
(2, 2) |
按匀速假设传播状态 |
H |
(1, 2) |
用 [[1.0, 0.0]] 取出位置 |
Q |
(2, 2) |
每个采样间隔新增的过程噪声协方差 |
R |
(1, 1) |
单个位置读数的噪声方差 |
以后接入测量时,1.2 米的读数可表示为 np.array([[1.2]], dtype=np.float64),其形状为 (1, 1)。
用标准差确定协方差
初始位置标准差取 5 米,速度标准差取 2 米每秒,并假定二者误差不相关,因此 P 的对角项为 25 和 4,非对角项为 0。x 的两个零只是均值猜测;把 P 也设为零,会表达初始状态没有误差。
位置传感器噪声假定为零均值,标准差设为 2 米,所以 R 取 [[4.0]]。协方差存的是方差或交叉协方差:P、Q 的位置和速度对角项单位分别为 m²、m²/s²,交叉项为 m²/s;R 的单位是 m²。
过程噪声采用零均值随机加速度,标准差 米每二次方秒。它在每个采样区间内恒定,不同区间独立,并独立于初始误差和测量噪声。一个加速度同时产生位置、速度扰动,故 Q 通常有非零交叉项:
这是离散随机加速度模型的 Q;连续白噪声加速度模型使用另一组时间幂次系数,不能直接混用。genui{"citation":{"ref":"turn4view0"}}
初始化并完成一次数值验收
下面把状态均值和协方差分成两条路径。F 负责时间传播,H 把状态投影为位置读数;S 是创新协方差,表示实际读数与预测读数之差的不确定性。此处只试算预测与投影,以检查矩阵能否衔接。genui{"citation":{"ref":"turn2view3"}}

以下完整脚本已在 Python 3.12、NumPy 2.3.5 中验证。在已安装 NumPy 的环境直接运行,无需专用滤波库。
import numpy as np
dt = 0.1
sigma_p0, sigma_v0 = 5.0, 2.0
sigma_z, sigma_a = 2.0, 1.0
# t=0 的先验;第二行是速度,零均值不表示速度已知。
x = np.array([[0.0], [0.0]], dtype=np.float64)
P = np.diag(np.array([sigma_p0**2, sigma_v0**2], dtype=np.float64))
F = np.array([[1.0, dt], [0.0, 1.0]], dtype=np.float64)
H = np.array([[1.0, 0.0]], dtype=np.float64)
R = np.array([[sigma_z**2]], dtype=np.float64)
# 每段共享一个随机加速度,使位置、速度扰动相关。
G = np.array([[0.5 * dt**2], [dt]], dtype=np.float64)
Q = sigma_a**2 * (G @ G.T)
matrices = {"x": x, "P": P, "F": F, "H": H, "Q": Q, "R": R}
shapes = {
"x": (2, 1), "P": (2, 2), "F": (2, 2),
"H": (1, 2), "Q": (2, 2), "R": (1, 1),
}
for name, A in matrices.items():
assert A.shape == shapes[name], f"{name}: shape mismatch"
assert A.dtype == np.float64, f"{name}: dtype mismatch"
assert np.isfinite(A).all(), f"{name}: non-finite value"
# 先检查对称,再检查半正定;此容差适用于本例尺度。
for name in ("P", "Q", "R"):
A = matrices[name]
assert np.allclose(A, A.T, rtol=0.0, atol=1e-12), f"{name}: asymmetric"
assert np.linalg.eigvalsh(A).min() >= -1e-12, f"{name}: not PSD"
assert R[0, 0] > 0.0
# 试算 t=0.1 s 的预测以验收矩阵,不覆盖初始 x、P。
x_pred = F @ x
P_pred = F @ P @ F.T + Q
z_pred = H @ x_pred
S = H @ P_pred @ H.T + R
assert x_pred.shape == (2, 1) and P_pred.shape == (2, 2)
assert z_pred.shape == (1, 1) and S.shape == (1, 1)
np.set_printoptions(precision=6, suppress=True, floatmode="fixed")
for name, A in matrices.items():
print(f"{name}: shape={A.shape}, dtype={A.dtype}")
print("Q:")
print(Q)
print("P_pred:")
print(P_pred)
print(f"z_pred={z_pred[0, 0]:.6f}, S={S[0, 0]:.6f}")
print("初始化检查通过")预期输出:
x: shape=(2, 1), dtype=float64
P: shape=(2, 2), dtype=float64
F: shape=(2, 2), dtype=float64
H: shape=(1, 2), dtype=float64
Q: shape=(2, 2), dtype=float64
R: shape=(1, 1), dtype=float64
Q:
[[0.000025 0.000500]
[0.000500 0.010000]]
P_pred:
[[25.040025 0.400500]
[ 0.400500 4.010000]]
z_pred=0.000000, S=29.040025
初始化检查通过验收时,位置预测方差从 25 增为 25.040025;创新方差再加上测量噪声方差 4,得到 29.040025。初始均值全为零,因此预测位置仍为零;这不表示预测误差也为零。
协方差必须对称且半正定。本例 Q 只有一个独立噪声来源,允许有零特征值,不应要求所有特征值严格为正。np.linalg.eigvalsh 按实对称矩阵求特征值,因此代码先显式检查对称性;1e-12 只是本例尺度下的舍入容差。genui{"citation":{"ref":"turn2view2"}}
接入循环前排除三个错误
- 混用一维数组和列向量。
np.array([0.0, 0.0]).T仍是(2,),与(2, 1)相加会广播成(2, 2),可能不报错。保留双层方括号,并检查运算结果的形状。genui{"citation":{"ref":"turn2view1"}} - 重复使用首个读数。 若改用 的位置测量初始化
x,并按其噪声设置P,应把这条测量视为已使用。若保留独立先验并收到 测量,则先在同一时刻更新,不能先推进到 0.1 秒。 - 改变采样间隔却只修改 F。 若间隔改为 0.2 秒,在同一随机加速度假设下,Q 的两个对角项应变为 0.0004、0.04,非对角项为 0.004。应同时重建 F 和 Q。
脚本保留了初始 x、P,试算结果另存于 x_pred、P_pred。接入正式循环时从约定时间戳开始,并将每次更新后的状态与协方差保留到下一轮;初始化只执行一次,不能放在循环内部反复重置。