自动控制 / 系统课时

初始化状态估计和协方差矩阵

初始化状态估计和协方差矩阵

滤波循环开始前,初始状态、误差尺度和模型矩阵必须使用同一套变量顺序。本篇用一维位置与速度估计构造完整的 NumPy 初始化代码,并检查形状、浮点类型和协方差,得到可以接入后续预测与更新的起点。

学习目标

  • 建立位置、速度顺序固定的列向量,匹配六个核心数组的形状。
  • 将初始误差与噪声的标准差转换为协方差,区分 P、Q、R。
  • 运行数值检查,识别广播错误、非法协方差和首测量重复使用。

先固定状态顺序和初始时刻

本例用 x^=[p,v]T\hat{x}=[p,v]^T 表示状态估计,位置单位为米,速度单位为米每秒。采样间隔为 0.1 秒,初值对应 t=0t=0;第一条待处理测量位于 t=0.1t=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²。

过程噪声采用零均值随机加速度,标准差 σa=1\sigma_a=1 米每二次方秒。它在每个采样区间内恒定,不同区间独立,并独立于初始误差和测量噪声。一个加速度同时产生位置、速度扰动,故 Q 通常有非零交叉项:

G=[Δt2/2Δt],Q=σa2GGT.G=\begin{bmatrix}\Delta t^2/2\\\Delta t\end{bmatrix},\qquad Q=\sigma_a^2GG^T.

这是离散随机加速度模型的 Q;连续白噪声加速度模型使用另一组时间幂次系数,不能直接混用。genui{"citation":{"ref":"turn4view0"}}

初始化并完成一次数值验收

下面把状态均值和协方差分成两条路径。F 负责时间传播,H 把状态投影为位置读数;S 是创新协方差,表示实际读数与预测读数之差的不确定性。此处只试算预测与投影,以检查矩阵能否衔接。genui{"citation":{"ref":"turn2view3"}}

状态均值与协方差分别经过预测和测量投影,Q、R 进入对应的不确定性计算

以下完整脚本已在 Python 3.12、NumPy 2.3.5 中验证。在已安装 NumPy 的环境直接运行,无需专用滤波库。

python
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("初始化检查通过")

预期输出:

text
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"}}

接入循环前排除三个错误

  1. 混用一维数组和列向量。 np.array([0.0, 0.0]).T 仍是 (2,),与 (2, 1) 相加会广播成 (2, 2),可能不报错。保留双层方括号,并检查运算结果的形状。genui{"citation":{"ref":"turn2view1"}}
  2. 重复使用首个读数。 若改用 t=0t=0 的位置测量初始化 x,并按其噪声设置 P,应把这条测量视为已使用。若保留独立先验并收到 t=0t=0 测量,则先在同一时刻更新,不能先推进到 0.1 秒。
  3. 改变采样间隔却只修改 F。 若间隔改为 0.2 秒,在同一随机加速度假设下,Q 的两个对角项应变为 0.0004、0.04,非对角项为 0.004。应同时重建 F 和 Q。

脚本保留了初始 x、P,试算结果另存于 x_pred、P_pred。接入正式循环时从约定时间戳开始,并将每次更新后的状态与协方差保留到下一轮;初始化只执行一次,不能放在循环内部反复重置。

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

目录与阅读设置

阅读设置

17

记录一个想法

初始化状态估计和协方差矩阵

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

登录哪都通