自动控制 / 系统课时
用 NumPy 编写状态预测函数
用 NumPy 编写状态预测函数
预测函数要同时推进位置、速度及它们的不确定性。本文把线性预测公式写成一个不修改输入的 NumPy 函数,通过一秒内的已知加速度算例,核对控制增量、协方差传播和数组形状,得到可接入滤波循环的预测结果。
学习目标
- 统一状态与控制的列向量形状,避免隐式广播产生错误矩阵。
- 实现均值和协方差预测,算出下一时刻的位置 4 m、速度 3 m/s。
- 使用手算值和数值断言验收函数,区分已知控制与过程噪声。
固定函数的输入与输出
x、P 表示上一时刻后验的均值与误差协方差,返回的 x_pred、P_pred 表示本时刻尚未用测量修正的先验。状态按 排列,位置在前、速度在后;本例状态维数 ,控制维数 。
| 参数 | 数组形状 | 含义 |
|---|---|---|
x |
(n, 1) |
上次状态估计 |
P |
(n, n) |
上次估计误差协方差 |
F |
(n, n) |
本次间隔的状态转移矩阵 |
Q |
(n, n) |
本次间隔的过程噪声协方差 |
B |
(n, m) |
控制输入到状态增量的映射 |
u |
(m, 1) |
本次间隔内的已知控制输入 |
输入中的 P、Q 必须对称、半正定,且所有数组都应包含有限实数。函数检查形状与有限值,协方差的数学有效性由调用方保证;主示例另用断言验收这一点。
确定一秒预测的数值模型
假设过程扰动零均值,并与上一时刻的估计误差独立;控制输入视为已知。均值与协方差分别按下面两式传播, 表示从 到 期间保持的输入。
取采样间隔 1 s,上次位置 2 m、速度 1 m/s,已知加速度 2 m/s²。初始位置方差为 4 m²,速度方差为 1 (m/s)²,互协方差为 0。由运动学关系,F 的第一行是 [1.0, dt],B 的两行分别为 和 。
过程扰动采用每段内恒定、各段之间独立的随机加速度,方差取 0.2 (m/s²)²。因此 ;这里噪声与控制恰好具有相同的输入方向,一般模型未必如此。这是离散随机加速度模型。genui{"citation":{"ref":"turn1view3"}}

实现并运行预测函数
示例在 Python 3.12、NumPy 2.3.5 下验证。使用已安装 NumPy 的环境,将完整代码保存为 predict_demo.py,执行 python predict_demo.py。本例使用普通 ndarray,矩阵乘法写成 @;* 是逐元素乘法,也可用于标量缩放。genui{"citation":{"refs":["turn1view0","turn1view1"]}}
import numpy as np
def predict(x, P, F, Q, B, u):
# 统一使用浮点数组和列向量,避免隐式广播改变结果形状。
x, P, F, Q, B, u = [
np.asarray(a, dtype=float) for a in (x, P, F, Q, B, u)
]
if x.ndim != 2 or x.shape[1] != 1 or x.shape[0] == 0:
raise ValueError("x must have shape (n, 1), n > 0")
n = x.shape[0]
if any(a.shape != (n, n) for a in (P, F, Q)):
raise ValueError("P, F, Q must have shape (n, n)")
if B.ndim != 2 or B.shape[0] != n:
raise ValueError("B must have shape (n, m)")
if u.shape != (B.shape[1], 1):
raise ValueError("u must have shape (m, 1)")
if not all(np.isfinite(a).all() for a in (x, P, F, Q, B, u)):
raise ValueError("all inputs must be finite")
# 预测分布的均值与协方差,不向均值添加随机噪声样本。
x_pred = F @ x + B @ u
P_pred = F @ P @ F.T + Q
return x_pred, P_pred
def main():
dt = 1.0
x = np.array([[2.0], [1.0]])
P = np.diag([4.0, 1.0])
F = np.array([[1.0, dt], [0.0, 1.0]])
B = np.array([[0.5 * dt**2], [dt]])
u = np.array([[2.0]])
# 每段恒定的随机加速度方差,与已知控制加速度分开建模。
sigma_a2 = 0.2
Q = sigma_a2 * (B @ B.T)
x_pred, P_pred = predict(x, P, F, Q, B, u)
# 用独立手算值验收,而非在断言中重复预测公式。
np.testing.assert_allclose(
x_pred, [[4.0], [3.0]], rtol=0.0, atol=1e-12
)
np.testing.assert_allclose(
P_pred, [[5.05, 1.10], [1.10, 1.20]], rtol=0.0, atol=1e-12
)
np.testing.assert_array_equal(x, [[2.0], [1.0]])
np.testing.assert_array_equal(P, [[4.0, 0.0], [0.0, 1.0]])
# 这里验收协方差;通用函数不在每次预测中做特征值分解。
for covariance in (P, Q, P_pred):
np.testing.assert_allclose(
covariance, covariance.T, rtol=0.0, atol=1e-12
)
assert np.linalg.eigvalsh(covariance).min() >= -1e-12
print("x_pred =", x_pred.tolist())
print("P_pred =", P_pred.tolist())
print("checks passed")
if __name__ == "__main__":
main()预期输出:
x_pred = [[4.0], [3.0]]
P_pred = [[5.05, 1.1], [1.1, 1.2]]
checks passed用输出检查均值与协方差
先核对均值:模型外推得到位置 3 m、速度 1 m/s,控制再增加位置 1 m、速度 2 m/s,因此结果为 4 m 和 3 m/s。tolist() 仅用于显示;函数返回的 x_pred 仍是形状为 (2, 1) 的数组。
再核对协方差: 的位置方差为 5、速度方差为 1、互协方差为 1;加入 Q 后分别变为 5.05 m²、1.20 (m/s)²、1.10 m²/s。位置标准差应取 m,不能把方差直接读成标准差。
np.testing.assert_allclose 按容差比较数组。这里设置 rtol=0.0、atol=1e-12,适用于本算例的数值量级;更换单位或数量级时,应重新选择容差。协方差验收允许零特征值,不要求所有输入协方差都严格正定。genui{"citation":{"ref":"turn2search0"}}
| 错误做法 | 结果或原因 | 修正 |
|---|---|---|
用 F * P * F.T 传播协方差 |
形状可能正确,但逐元素乘法漏掉交叉贡献 | 使用 F @ P @ F.T |
把 x 写成形状 (2,) |
与 (2, 1) 的控制增量相加,会广播成 (2, 2) |
按列向量构造;一维数组的 .T 不会增加维数 |
| 给预测均值叠加随机噪声样本 | 把均值估计变成随机轨迹采样 | 零均值扰动通过 Q 进入协方差 |
修改控制输入并检查边界
把 u 改为 [[0.0]],并把状态断言的期望值改为 [[3.0], [1.0]],重新运行。此时控制增量消失,但 P_pred 仍应保持原数值。因为控制被当作确定量,它的大小不直接增加估计不确定性。
如果采样间隔改为 0.5 s,必须同步重建 F、B 和本噪声模型下的 Q,再重算断言期望值。控制为零也不意味着过程噪声为零;Q 描述未被已知输入解释的扰动。
对控制维数固定的稠密实现,协方差传播的主要时间成本为 ,存储成本为 。返回的两项结果用于同一时刻的测量更新;测量值和测量噪声协方差 R 不进入这个预测函数。