自动控制 / 系统课时
后验更新怎样修正均值和协方差
后验更新怎样修正均值和协方差
收到新的位置读数后,滤波器需要同时修正位置、速度及其不确定性。本文从估计误差推导两条后验更新公式,再用一个两维状态算例和完整 Python 脚本,验证一次预测与更新如何把观测纳入当前估计。
学习目标
- 根据先验、新息和增益,算出位置与速度的后验均值。
- 从误差表达式推导 Joseph 协方差更新,并说明简化公式的适用条件。
- 跑通一次预测与更新,检查协方差的对称性、半正定性及数值结果。
先验和后验对应同一时刻
状态取 ,单位分别为米和米/秒。记 为尚未使用本拍观测的先验, 为使用观测后的后验;对应误差协方差为 、。上标正负表示更新前后,不表示正负数。
位置传感器的模型为:
这里 是零均值测量噪声,协方差为 。本篇假设先验误差与当前测量噪声均为零均值高斯且相互独立, 正定;下文期望默认以此前观测为条件。这样更新得到的是高斯后验的均值和协方差。
例如,预测位置为 1 米、速度为 1 米/秒,读数为 2.5 米时,必须先计算 米的差。不能直接用一维位置读数覆盖两维状态,也不能把读数减去速度。
用新息推导均值修正
定义先验误差 ,把测量与预测读数的差称为新息:
是新息协方差,同时包含预测误差和传感器噪声。设修正量为 ,则剩余误差为 。这里先把 视为待确定的矩阵。
最小均方误差的线性估计要求剩余误差与已利用的新息不相关。代入上式,并利用两种误差独立,可得:
解出增益后,均值更新就是:
本例状态有 2 个分量、测量有 1 个分量,因此 为 ,把位置空间中的新息映射成位置和速度的修正。在上述高斯条件下,剩余误差与新息不相关也就意味着独立,所以这个估计也是条件均值。genui{"citation":{"ref":"turn2view2"}}
从剩余误差推导协方差
将测量模型代入后验误差:
两项分别来自尚未消除的先验误差和本次引入的测量噪声。计算 时,交叉项因独立且零均值而消失,得到 Joseph 形式:
是与状态维度相同的单位矩阵,本例为 。如果漏掉第二项,就等于忽略观测带来的噪声,通常会低估不确定性。
展开 Joseph 形式,利用 ,得到:
采用前面求出的最优增益时,,于是精确算术下可以简化为:
若人为调整增益,不能继续套用这个简式。代码采用 Joseph 形式:它把两项半正定矩阵相加,通常比相减简式更能抵抗浮点误差,但仍应检查数值结果。genui{"citation":{"ref":"turn1view0"}}
由于 半正定,同一时刻更新后的不确定性不会在任何线性方向上增加;不一定每个方向都严格减小,也不保证某一次估计必然更接近真实状态。
算清一次位置与速度更新
采样间隔为 1 秒,没有控制输入。上一拍后验为 、,采用匀速转移矩阵 和过程噪声协方差 :
是本例给定的离散状态过程噪声协方差,表示位置、速度分别受到独立扰动;它不是单一随机加速度模型离散化得到的矩阵。两个对角元素的单位分别为平方米、平方米/秒平方。
先完成预测:
取位置读数 米、测量方差 平方米,可得:
因此位置修正 米、速度修正 米/秒,结果如下:
| 检查量 | 更新前 | 更新后 |
|---|---|---|
| 位置均值(米) | 1 | 2.1 |
| 速度均值(米/秒) | 1 | 1.5 |
| 位置方差(平方米) | 2.2 | 0.586667 |
| 速度方差(平方米/秒平方) | 1.1 | 0.766667 |
| 位置与速度误差协方差(平方米/秒) | 1 | 0.266667 |
速度能被修正,是因为先验位置与速度误差的交叉协方差为 1,速度增益为 。传感器并没有直接测量速度;如果这一刻交叉协方差为零,这次位置更新就不会改变速度均值。

运行完整循环并检查边界
在已安装 NumPy 的 Python 3 环境中运行以下完整脚本。状态和测量用一维数组保存,、、、 保持二维,避免列向量与一维数组混用引发广播错误。np.linalg.solve 解线性方程,不显式计算逆矩阵。genui{"citation":{"ref":"turn2view1"}}
import numpy as np
np.set_printoptions(precision=6, suppress=True)
x_prev = np.array([0.0, 1.0])
P_prev = np.diag([1.0, 1.0])
F = np.array([[1.0, 1.0], [0.0, 1.0]])
Q = np.diag([0.2, 0.1])
H = np.array([[1.0, 0.0]])
R = np.array([[0.8]])
z = np.array([2.5])
# 预测尚未使用本拍读数;保留先验供两项更新共同使用。
x_prior = F @ x_prev
P_prior = F @ P_prev @ F.T + Q
innovation = z - H @ x_prior
S = H @ P_prior @ H.T + R
# S 与 P_prior 对称,转置求解可得到所需的增益形状。
K = np.linalg.solve(S, H @ P_prior).T
x_post = x_prior + K @ innovation
# Joseph 形式保留先验误差与测量噪声的两项贡献。
A = np.eye(2) - K @ H
P_post = A @ P_prior @ A.T + K @ R @ K.T
# 检查代数性质;1e-12 允许本例量级下的浮点舍入误差。
assert np.allclose(P_post, P_post.T)
assert np.linalg.eigvalsh(P_post).min() >= -1e-12
assert np.linalg.eigvalsh(P_prior - P_post).min() >= -1e-12
assert np.allclose(P_post, A @ P_prior)
print("x_prior =", x_prior)
print("P_prior =\n", P_prior, sep="")
print("innovation =", innovation)
print("S =", S)
print("K =", K.ravel())
print("x_post =", x_post)
print("P_post =\n", P_post, sep="")
print("checks = OK")预期输出:
x_prior = [1. 1.]
P_prior =
[[2.2 1. ]
[1. 1.1]]
innovation = [1.5]
S = [[3.]]
K = [0.733333 0.333333]
x_post = [2.1 1.5]
P_post =
[[0.586667 0.266667]
[0.266667 0.766667]]
checks = OK四个断言依次检查对称性、后验半正定、同拍协方差不增,以及最优增益下 Joseph 形式与简式一致。固定状态维度后,每拍只保留当前均值和协方差,存储量不随历史观测数量增长。
把读数改为 1 米时,新息变为零,后验均值仍为 ,但后验协方差保持表中结果。这是因为固定 、、 后,协方差公式不含读数;新观测与预测一致,仍提供了信息。
若这拍没有有效读数,应跳过测量更新,令后验等于先验;不要伪造零新息再缩小协方差。同一条观测也不能重复作为独立证据更新,否则会过度自信。下一拍从本拍后验重新预测,加入 后不确定性仍可能增大。