自动控制 / 系统课时
预测协方差为何要加入过程噪声
预测协方差为何要加入过程噪声
预测位置和速度时,仅传播上一时刻的估计误差,还没有计算这一秒内新发生的随机扰动。本篇从误差递推推导预测协方差公式,用具体矩阵区分模型传播与过程噪声的贡献,再用 NumPy 验证遗漏过程噪声会低估多少不确定性。
学习目标
- 从状态误差推导预测协方差公式,并说出交叉项消失的条件。
- 用一秒采样的随机加速度模型构造过程噪声协方差。
- 运行矩阵算例,核对单步预测与连续五步预测的结果。
从估计误差推导协方差
用 表示位置和速度。上标 表示融合测量后的后验,上标 表示尚未融合当前测量的先验。设 为转移矩阵, 为准确已知的控制增量, 为本步过程扰动:
误差定义为真实状态减估计状态,即 。两式相减,已知控制增量抵消,得到 。因此,预测误差包含传播来的旧误差,以及本步新加入的扰动。
下面的期望都以截至 的测量为条件。假设旧误差与 均为零均值,且二者不相关;定义 、。于是:
展开乘积时,两项交叉期望因不相关而为零;右侧的转置来自误差外积的转置运算。若旧误差与本步噪声相关,就不能直接删掉交叉项。该递推式可对照 FilterPy 的标准预测实现。genui{"citation":{"ref":"turn2view1"}}
看清模型传播改变了什么
令采样间隔 ,采用匀速模型。上一后验的位置标准差为 ,速度标准差为 ,两种误差不相关,因此协方差对角线分别填平方后的 和 :
暂时只传播旧误差,计算得到:
位置方差从 变为 ,因为旧速度误差会在这一秒内积累为位移误差。非对角项从 变为 ,因为同一份速度误差同时进入了预测位置和预测速度;速度方差仍为 。
这些变化都来自已有误差,尚未包含新的加速度扰动。矩阵左上角单位为 ,右下角为 ,非对角项为 ;不能把整张矩阵当成一种单位的标量。
把随机加速度换算为过程噪声
假设每个采样区间内存在一个恒定的未知加速度 ,不同区间彼此独立,并与上一后验误差不相关。它的均值为 ,方差为 。同一次扰动会造成位置增量 和速度增量 。
令 ,则 ,在状态空间中的噪声协方差为:
这里的 是加速度方差,不能直接当作位置或速度方差。该模型与 FilterPy 的离散白噪声构造 一致;连续时间白加速度模型采用不同的离散化,不能混用参数。genui{"citation":{"ref":"turn2view0"}}
取 ,代入 ,再与模型传播结果相加:
的非对角项 来自位置和速度共享同一个随机加速度。虽然扰动均值为零、不会给预测均值增加确定偏移,但它仍会扩大预测误差的分布,不能因此省略 。

用 NumPy 验证单步与连续预测
在已安装 NumPy 的 Python 3 环境中直接运行下面的完整脚本。前半段复算三个矩阵,后半段从同一个初始协方差出发,连续预测五次且不做测量更新,以便单独观察过程噪声的累计贡献。
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")预期输出:
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单步预测的位置、速度标准差约为 和 。五步后,忽略 的位置方差仍从 增至 ,但正确计入扰动后应为 ;速度方差也应从 增至 。
检查过程噪声是否加对
- 矩阵乘法用对运算符。 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。
“加入 ”不表示每个矩阵元素都必须严格变大。本例的 有一个零特征值,仍是合法的半正定协方差;它保证相对 不减少任一固定方向的误差方差,也不保证 一定大于上一时刻的 。
准确已知的控制增量不会直接增加协方差;执行误差应按实际扰动模型计入 。只有模型确实没有新增随机扰动时,令 才与假设一致;存在扰动却省略它,会把预测结果报告得过于确定。