自动控制 / 系统课时

用 NumPy 编写观测更新函数

用 NumPy 编写观测更新函数

把预测位置与新读数融合,需要同时更新状态均值和协方差。本篇实现一个不修改输入数组的 NumPy 观测更新函数,用预测位置 2 m、速度 1 m/s 和位置读数 3 m 的算例,核对新息、增益与后验结果。

学习目标

  • 约定状态、观测和矩阵的形状,避免一维数组与列向量混用。
  • 用 np.linalg.solve 计算增益,并用 Joseph 形式更新协方差。
  • 通过手算和断言,核对后验位置 2.8 m、速度 1.2 m/s 及协方差。

先固定输入和数组形状

函数 update(x_pred, P_pred, z, H, R) 接收已经预测到观测时刻的先验。状态顺序固定为位置、速度;设状态维数为 nn、观测维数为 mm,本例 n=2n=2、m=1m=1。数学向量按列参与运算,在 NumPy 中统一存成一维数组。

参数 数组形状 本例取值
x_pred (n,) [2.0, 1.0]
P_pred (n, n) [[4.0, 1.0], [1.0, 1.0]]
z (m,) [3.0]
H (m, n) [[1.0, 0.0]]
R (m, m) [[1.0]]

观测模型为 z=Hx+vz=Hx+v,测量噪声 vv 均值为零、协方差为 RR,并与先验估计误差独立。这里约定 P_pred 对称半正定、R 对称正定;H 只读取位置,但更新结果仍包含位置和速度。

R 存方差而非标准差。本例位置测量标准差为 1 m,因此方差为 1 m²;若标准差改为 2 m,应传入 [[4.0]]。即使只有一个读数,z 仍用一维数组,H、R 仍用二维数组。

先手算一次位置观测

用上标 −- 表示先验、++ 表示本次观测后的后验。先将状态映射到测量空间,再算新息 yy 和新息协方差 SS;随后计算增益 KK,把新息映射回状态空间:

y=z−Hx−,S=HP−HT+Ry=z-Hx^-,\qquad S=HP^-H^T+R
K=P−HTS−1,x+=x−+KyK=P^-H^TS^{-1},\qquad x^+=x^-+Ky

代入本例,预测读数为 2 m,新息为 3−2=13-2=1 m,新息方差为 4+1=54+1=5 m²。增益为 K=[0.8, 0.2]TK=[0.8,\,0.2]^T,于是位置增加 0.8 m、速度增加 0.2 m/s,后验均值为 [2.8, 1.2]T[2.8,\,1.2]^T。

协方差采用 Joseph 形式,数值稳定性通常优于直接计算 P+=(I−KH)P−P^+=(I-KH)P^-;FilterPy 公开实现也采用这一形式。genui{"citation":{"ref":"turn1view1"}}

P+=(I−KH)P−(I−KH)T+KRKTP^+=(I-KH)P^-(I-KH)^T+KRK^T

两项贡献的位置方差分别为 0.22×4=0.160.2^2\times4=0.16 和 0.82×1=0.640.8^2\times1=0.64,合计 0.8 m²。漏掉第二项就会低估不确定性。完整后验协方差为 [[0.8, 0.2], [0.2, 0.8]],下面逐项核对。

新息修正状态均值,先验协方差与测量噪声共同决定增益和后验协方差

写成可复用的 NumPy 函数

NumPy 官方文档规定 np.linalg.solve(a, b) 求解 aX=baX=b,系数矩阵必须为满秩方阵。这里将增益计算改写成 STKT=(P−HT)TS^TK^T=(P^-H^T)^T,求解后再转置回来。genui{"citation":{"ref":"turn1view0"}}

下例已在 Python 3.12.14、NumPy 2.3.5 环境运行。代码只依赖 NumPy,可整段执行;函数依次返回后验均值、后验协方差、新息、新息协方差和增益。矩阵乘法使用 @,不能换成逐元素相乘的 *。

python
import numpy as np

def update(x_pred, P_pred, z, H, R):
    # 统一浮点类型,但不把错误形状自动展平,以免掩盖输入问题。
    x_pred = np.asarray(x_pred, dtype=float)
    P_pred = np.asarray(P_pred, dtype=float)
    z = np.asarray(z, dtype=float)
    H = np.asarray(H, dtype=float)
    R = np.asarray(R, dtype=float)
    if x_pred.ndim != 1 or z.ndim != 1:
        raise ValueError("x_pred and z must be 1-D")
    n, m = x_pred.size, z.size
    if P_pred.shape != (n, n) or H.shape != (m, n) or R.shape != (m, m):
        raise ValueError("incompatible matrix shapes")
    if not all(np.isfinite(a).all() for a in (x_pred, P_pred, z, H, R)):
        raise ValueError("inputs must be finite")

    # 新息及其协方差都在测量空间计算,全程使用同一份先验。
    innovation = z - H @ x_pred
    PHt = P_pred @ H.T
    S = H @ PHt + R

    # 解 S.T @ K.T = PHt.T,避免显式构造 S 的逆矩阵。
    K = np.linalg.solve(S.T, PHt.T).T
    x_post = x_pred + K @ innovation

    # Joseph 形式保留两个协方差项,减轻浮点消减误差。
    I_KH = np.eye(n) - K @ H
    P_post = I_KH @ P_pred @ I_KH.T + K @ R @ K.T
    P_post = 0.5 * (P_post + P_post.T)
    return x_post, P_post, innovation, S, K

if __name__ == "__main__":
    x_pred = np.array([2.0, 1.0])
    P_pred = np.array([[4.0, 1.0], [1.0, 1.0]])
    z = np.array([3.0])
    H = np.array([[1.0, 0.0]])
    R = np.array([[1.0]])

    x_post, P_post, innovation, S, K = update(x_pred, P_pred, z, H, R)

    # 与独立手算值核对,同时检查形状与协方差半正定性。
    np.testing.assert_allclose(x_post, [2.8, 1.2], rtol=0, atol=1e-12)
    np.testing.assert_allclose(
        P_post, [[0.8, 0.2], [0.2, 0.8]], rtol=0, atol=1e-12
    )
    assert x_post.shape == (2,) and P_post.shape == (2, 2)
    assert np.linalg.eigvalsh(P_post).min() >= -1e-12

    print("innovation =", innovation.round(6).tolist())
    print("S =", S.round(6).tolist())
    print("K =", K.round(6).tolist())
    print("x_post =", x_post.round(6).tolist())
    print("P_post =", P_post.round(6).tolist())
    print("checks passed")

预期输出:

text
innovation = [1.0]
S = [[5.0]]
K = [[0.8], [0.2]]
x_post = [2.8, 1.2]
P_post = [[0.8, 0.2], [0.2, 0.8]]
checks passed

检查结果和处理异常

只测位置仍能修正速度,是因为先验的位置与速度交叉协方差为 1,本例速度增益为 1/5=0.2 s−11/5=0.2\,\mathrm{s}^{-1}。若把先验两个非对角项改为零,保持其余输入不变,本次速度后验应仍为 1 m/s,可据此检查关联更新。

后验位置方差从 4 降到 0.8 m²,速度方差从 1 降到 0.8 (m/s)²。代码用 np.testing.assert_allclose 核对独立手算值,再检查形状和半正定性;这里的 1e-12 容差针对本例量级,不能无条件套到所有单位尺度。

  • 形状错误: (2,) 与 (2, 1) 相加可能广播成 (2, 2)。函数明确拒绝列向量输入,避免返回一个看似能算、实际错误的状态矩阵。
  • 求解失败: 有效半正定 P_pred 与正定 R 会使 S 正定。若出现 np.linalg.LinAlgError,先查噪声是否被误设为零、是否存在重复的零噪声观测,以及输入协方差是否合法,再处理模型问题。
  • 无效或缺失读数: 函数会拒绝 NaN 和无穷大。整次观测缺失时跳过更新,保留预测结果;不要用数值零代替缺测。

函数只检查形状和有限数值,输入协方差的合法性仍由调用者保证。最后的对称化用于消除微小舍入不对称,不能修复负方差。接入循环时,每份观测只融合一次,再把 x_post、P_post 交给下一拍预测;过程噪声 QQ 已在预测阶段计入,不要在此重复添加。

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

目录与阅读设置

阅读设置

17

记录一个想法

用 NumPy 编写观测更新函数

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

登录哪都通