自动控制 / 系统课时
用 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) 接收已经预测到观测时刻的先验。状态顺序固定为位置、速度;设状态维数为 、观测维数为 ,本例 、。数学向量按列参与运算,在 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]] |
观测模型为 ,测量噪声 均值为零、协方差为 ,并与先验估计误差独立。这里约定 P_pred 对称半正定、R 对称正定;H 只读取位置,但更新结果仍包含位置和速度。
R 存方差而非标准差。本例位置测量标准差为 1 m,因此方差为 1 m²;若标准差改为 2 m,应传入 [[4.0]]。即使只有一个读数,z 仍用一维数组,H、R 仍用二维数组。
先手算一次位置观测
用上标 表示先验、 表示本次观测后的后验。先将状态映射到测量空间,再算新息 和新息协方差 ;随后计算增益 ,把新息映射回状态空间:
代入本例,预测读数为 2 m,新息为 m,新息方差为 m²。增益为 ,于是位置增加 0.8 m、速度增加 0.2 m/s,后验均值为 。
协方差采用 Joseph 形式,数值稳定性通常优于直接计算 ;FilterPy 公开实现也采用这一形式。genui{"citation":{"ref":"turn1view1"}}
两项贡献的位置方差分别为 和 ,合计 0.8 m²。漏掉第二项就会低估不确定性。完整后验协方差为 [[0.8, 0.2], [0.2, 0.8]],下面逐项核对。

写成可复用的 NumPy 函数
NumPy 官方文档规定 np.linalg.solve(a, b) 求解 ,系数矩阵必须为满秩方阵。这里将增益计算改写成 ,求解后再转置回来。genui{"citation":{"ref":"turn1view0"}}
下例已在 Python 3.12.14、NumPy 2.3.5 环境运行。代码只依赖 NumPy,可整段执行;函数依次返回后验均值、后验协方差、新息、新息协方差和增益。矩阵乘法使用 @,不能换成逐元素相乘的 *。
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")预期输出:
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 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 交给下一拍预测;过程噪声 已在预测阶段计入,不要在此重复添加。