自动控制 / 系统课时

用 NumPy 编写状态预测函数

用 NumPy 编写状态预测函数

预测函数要同时推进位置、速度及它们的不确定性。本文把线性预测公式写成一个不修改输入的 NumPy 函数,通过一秒内的已知加速度算例,核对控制增量、协方差传播和数组形状,得到可接入滤波循环的预测结果。

学习目标

  • 统一状态与控制的列向量形状,避免隐式广播产生错误矩阵。
  • 实现均值和协方差预测,算出下一时刻的位置 4 m、速度 3 m/s。
  • 使用手算值和数值断言验收函数,区分已知控制与过程噪声。

固定函数的输入与输出

x、P 表示上一时刻后验的均值与误差协方差,返回的 x_pred、P_pred 表示本时刻尚未用测量修正的先验。状态按 x=[p,v]Tx=[p,v]^T 排列,位置在前、速度在后;本例状态维数 n=2n=2,控制维数 m=1m=1。

参数 数组形状 含义
x (n, 1) 上次状态估计
P (n, n) 上次估计误差协方差
F (n, n) 本次间隔的状态转移矩阵
Q (n, n) 本次间隔的过程噪声协方差
B (n, m) 控制输入到状态增量的映射
u (m, 1) 本次间隔内的已知控制输入

输入中的 P、Q 必须对称、半正定,且所有数组都应包含有限实数。函数检查形状与有限值,协方差的数学有效性由调用方保证;主示例另用断言验收这一点。

确定一秒预测的数值模型

假设过程扰动零均值,并与上一时刻的估计误差独立;控制输入视为已知。均值与协方差分别按下面两式传播,uku_k 表示从 k−1k-1 到 kk 期间保持的输入。

x^k∣k−1=Fkx^k−1∣k−1+Bkuk\hat{x}_{k|k-1}=F_k\hat{x}_{k-1|k-1}+B_ku_k
Pk∣k−1=FkPk−1∣k−1FkT+QkP_{k|k-1}=F_kP_{k-1|k-1}F_k^T+Q_k

取采样间隔 1 s,上次位置 2 m、速度 1 m/s,已知加速度 2 m/s²。初始位置方差为 4 m²,速度方差为 1 (m/s)²,互协方差为 0。由运动学关系,F 的第一行是 [1.0, dt],B 的两行分别为 0.5Δt20.5\Delta t^2 和 Δt\Delta t。

过程扰动采用每段内恒定、各段之间独立的随机加速度,方差取 0.2 (m/s²)²。因此 Q=σa2BBTQ=\sigma_a^2BB^T;这里噪声与控制恰好具有相同的输入方向,一般模型未必如此。这是离散随机加速度模型。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"]}}

python
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()

预期输出:

text
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) 的数组。

再核对协方差:FPFTFPF^T 的位置方差为 5、速度方差为 1、互协方差为 1;加入 Q 后分别变为 5.05 m²、1.20 (m/s)²、1.10 m²/s。位置标准差应取 5.05≈2.247\sqrt{5.05}\approx2.247 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 描述未被已知输入解释的扰动。

对控制维数固定的稠密实现,协方差传播的主要时间成本为 O(n3)O(n^3),存储成本为 O(n2)O(n^2)。返回的两项结果用于同一时刻的测量更新;测量值和测量噪声协方差 R 不进入这个预测函数。

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

目录与阅读设置

阅读设置

17

记录一个想法

用 NumPy 编写状态预测函数

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

登录哪都通