自动控制 / 系统课时

预测协方差为何要加入过程噪声

预测协方差为何要加入过程噪声

预测位置和速度时,仅传播上一时刻的估计误差,还没有计算这一秒内新发生的随机扰动。本篇从误差递推推导预测协方差公式,用具体矩阵区分模型传播与过程噪声的贡献,再用 NumPy 验证遗漏过程噪声会低估多少不确定性。

学习目标

  • 从状态误差推导预测协方差公式,并说出交叉项消失的条件。
  • 用一秒采样的随机加速度模型构造过程噪声协方差。
  • 运行矩阵算例,核对单步预测与连续五步预测的结果。

从估计误差推导协方差

用 xk=[pk,vk]Tx_k=[p_k,v_k]^T 表示位置和速度。上标 ++ 表示融合测量后的后验,上标 −- 表示尚未融合当前测量的先验。设 FkF_k 为转移矩阵,BkukB_ku_k 为准确已知的控制增量,wkw_k 为本步过程扰动:

xk=Fkxk−1+Bkuk+wk,x^k−=Fkx^k−1++Bkuk.\begin{aligned} x_k&=F_kx_{k-1}+B_ku_k+w_k,\\ \hat{x}_k^-&=F_k\hat{x}_{k-1}^++B_ku_k. \end{aligned}

误差定义为真实状态减估计状态,即 ek−1+=xk−1−x^k−1+e_{k-1}^+=x_{k-1}-\hat{x}_{k-1}^+。两式相减,已知控制增量抵消,得到 ek−=Fkek−1++wke_k^-=F_ke_{k-1}^++w_k。因此,预测误差包含传播来的旧误差,以及本步新加入的扰动。

下面的期望都以截至 k−1k-1 的测量为条件。假设旧误差与 wkw_k 均为零均值,且二者不相关;定义 Pk−1+=E[ek−1+(ek−1+)T]P_{k-1}^+=\mathbb E[e_{k-1}^+(e_{k-1}^+)^T]、Qk=E[wkwkT]Q_k=\mathbb E[w_kw_k^T]。于是:

Pk−=E[ek−(ek−)T]=E[(Fkek−1++wk)(Fkek−1++wk)T]=FkPk−1+FkT+Qk.\begin{aligned} P_k^-&=\mathbb E[e_k^-(e_k^-)^T]\\ &=\mathbb E[(F_ke_{k-1}^++w_k)(F_ke_{k-1}^++w_k)^T]\\ &=F_kP_{k-1}^+F_k^T+Q_k. \end{aligned}

展开乘积时,两项交叉期望因不相关而为零;右侧的转置来自误差外积的转置运算。若旧误差与本步噪声相关,就不能直接删掉交叉项。该递推式可对照 FilterPy 的标准预测实现。genui{"citation":{"ref":"turn2view1"}}

看清模型传播改变了什么

令采样间隔 Δt=1 s\Delta t=1\,\mathrm{s},采用匀速模型。上一后验的位置标准差为 2 m2\,\mathrm m,速度标准差为 1 m/s1\,\mathrm{m/s},两种误差不相关,因此协方差对角线分别填平方后的 44 和 11:

F=[1101],P+=[4001].F=\begin{bmatrix}1&1\\0&1\end{bmatrix}, \qquad P^+=\begin{bmatrix}4&0\\0&1\end{bmatrix}.

暂时只传播旧误差,计算得到:

FP+FT=[5111].FP^+F^T=\begin{bmatrix}5&1\\1&1\end{bmatrix}.

位置方差从 44 变为 55,因为旧速度误差会在这一秒内积累为位移误差。非对角项从 00 变为 11,因为同一份速度误差同时进入了预测位置和预测速度;速度方差仍为 11。

这些变化都来自已有误差,尚未包含新的加速度扰动。矩阵左上角单位为 m2\mathrm{m^2},右下角为 m2/s2\mathrm{m^2/s^2},非对角项为 m2/s\mathrm{m^2/s};不能把整张矩阵当成一种单位的标量。

把随机加速度换算为过程噪声

假设每个采样区间内存在一个恒定的未知加速度 aka_k,不同区间彼此独立,并与上一后验误差不相关。它的均值为 00,方差为 σa2\sigma_a^2。同一次扰动会造成位置增量 12akΔt2\tfrac12a_k\Delta t^2 和速度增量 akΔta_k\Delta t。

令 G=[Δt2/2,Δt]TG=[\Delta t^2/2,\Delta t]^T,则 wk=Gakw_k=Ga_k,在状态空间中的噪声协方差为:

Q=σa2GGT=σa2[Δt4/4Δt3/2Δt3/2Δt2].Q=\sigma_a^2GG^T =\sigma_a^2 \begin{bmatrix} \Delta t^4/4&\Delta t^3/2\\ \Delta t^3/2&\Delta t^2 \end{bmatrix}.

这里的 σa2\sigma_a^2 是加速度方差,不能直接当作位置或速度方差。该模型与 FilterPy 的离散白噪声构造 一致;连续时间白加速度模型采用不同的离散化,不能混用参数。genui{"citation":{"ref":"turn2view0"}}

取 σa2=0.4 m2/s4\sigma_a^2=0.4\,\mathrm{m^2/s^4},代入 Δt=1 s\Delta t=1\,\mathrm{s},再与模型传播结果相加:

Q=[0.10.20.20.4],P−=[5.11.21.21.4].Q=\begin{bmatrix}0.1&0.2\\0.2&0.4\end{bmatrix}, \qquad P^-=\begin{bmatrix}5.1&1.2\\1.2&1.4\end{bmatrix}.

QQ 的非对角项 0.20.2 来自位置和速度共享同一个随机加速度。虽然扰动均值为零、不会给预测均值增加确定偏移,但它仍会扩大预测误差的分布,不能因此省略 QQ。

旧误差经模型传播后,与本步过程噪声协方差相加形成预测协方差

用 NumPy 验证单步与连续预测

在已安装 NumPy 的 Python 3 环境中直接运行下面的完整脚本。前半段复算三个矩阵,后半段从同一个初始协方差出发,连续预测五次且不做测量更新,以便单独观察过程噪声的累计贡献。

python
import numpy as np

np.set_printoptions(precision=3, suppress=True)
dt = 1.0
F = np.array([[1.0, dt], [0.0, 1.0]])
P_post = np.diag([4.0, 1.0])

# 同一个随机加速度同时影响位置和速度,保留相关项。
G = np.array([[0.5 * dt**2], [dt]])
Q = 0.4 * (G @ G.T)
P_model = F @ P_post @ F.T
P_pred = P_model + Q

# 检查协方差的对称性、半正定性和新增噪声贡献。
assert np.allclose(P_pred, P_pred.T)
assert np.linalg.eigvalsh(P_pred).min() >= -1e-12
assert np.allclose(P_pred - P_model, Q)
for name, matrix in (("P_model", P_model), ("Q", Q), ("P_pred", P_pred)):
    print(name + ":")
    print(matrix)
print("std:", np.sqrt(np.diag(P_pred)))

# 不做测量更新,比较五个采样区间的误差累积。
for label, step_Q in (("Q=0", np.zeros_like(Q)), ("with Q", Q)):
    P = P_post.copy()
    for _ in range(5):
        P = F @ P @ F.T + step_Q
    print(label + ", 5 predictions:")
    print(P)
print("checks: passed")

预期输出:

text
P_model:
[[5. 1.]
 [1. 1.]]
Q:
[[0.1 0.2]
 [0.2 0.4]]
P_pred:
[[5.1 1.2]
 [1.2 1.4]]
std: [2.258 1.183]
Q=0, 5 predictions:
[[29.  5.]
 [ 5.  1.]]
with Q, 5 predictions:
[[45.5 10. ]
 [10.   3. ]]
checks: passed

单步预测的位置、速度标准差约为 2.258 m2.258\,\mathrm m 和 1.183 m/s1.183\,\mathrm{m/s}。五步后,忽略 QQ 的位置方差仍从 44 增至 2929,但正确计入扰动后应为 45.545.5;速度方差也应从 11 增至 33。

检查过程噪声是否加对

  • 矩阵乘法用对运算符。 NumPy 中应写 F @ P @ F.T + Q。对数组使用 F * P * F.T 会逐元素相乘,得不到协方差传播结果。
  • 先确认噪声在哪个空间。 本例的 Q 已经是位置、速度空间中的协方差,应直接相加;只有尚未映射的加速度方差才需要通过 G 变换,不能再把成品 Q 映射一次。
  • 修改采样周期时同步重算。 把 dt 改为 0.5,保持加速度方差为 0.4,则 Q[0, 0]、Q[0, 1]、Q[1, 1] 应分别变成 0.00625、0.025、0.1。

“加入 QQ”不表示每个矩阵元素都必须严格变大。本例的 QQ 有一个零特征值,仍是合法的半正定协方差;它保证相对 FP+FTFP^+F^T 不减少任一固定方向的误差方差,也不保证 P−P^- 一定大于上一时刻的 P+P^+。

准确已知的控制增量不会直接增加协方差;执行误差应按实际扰动模型计入 QQ。只有模型确实没有新增随机扰动时,令 Q=0Q=0 才与假设一致;存在扰动却省略它,会把预测结果报告得过于确定。

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

目录与阅读设置

阅读设置

17

记录一个想法

预测协方差为何要加入过程噪声

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

登录哪都通