自动控制 / 系统课时

后验更新怎样修正均值和协方差

后验更新怎样修正均值和协方差

收到新的位置读数后,滤波器需要同时修正位置、速度及其不确定性。本文从估计误差推导两条后验更新公式,再用一个两维状态算例和完整 Python 脚本,验证一次预测与更新如何把观测纳入当前估计。

学习目标

  • 根据先验、新息和增益,算出位置与速度的后验均值。
  • 从误差表达式推导 Joseph 协方差更新,并说明简化公式的适用条件。
  • 跑通一次预测与更新,检查协方差的对称性、半正定性及数值结果。

先验和后验对应同一时刻

状态取 xk=[pk,vk]Tx_k=[p_k,v_k]^T,单位分别为米和米/秒。记 x^k−=x^k∣k−1\hat x_k^-=\hat x_{k|k-1} 为尚未使用本拍观测的先验,x^k+=x^k∣k\hat x_k^+=\hat x_{k|k} 为使用观测后的后验;对应误差协方差为 Pk−P_k^-、Pk+P_k^+。上标正负表示更新前后,不表示正负数。

位置传感器的模型为:

zk=Hkxk+εk,Hk=[10].z_k=H_kx_k+\varepsilon_k,\qquad H_k=\begin{bmatrix}1&0\end{bmatrix}.

这里 εk\varepsilon_k 是零均值测量噪声,协方差为 RkR_k。本篇假设先验误差与当前测量噪声均为零均值高斯且相互独立,RkR_k 正定;下文期望默认以此前观测为条件。这样更新得到的是高斯后验的均值和协方差。

例如,预测位置为 1 米、速度为 1 米/秒,读数为 2.5 米时,必须先计算 2.5−1=1.52.5-1=1.5 米的差。不能直接用一维位置读数覆盖两维状态,也不能把读数减去速度。

用新息推导均值修正

定义先验误差 ek−=xk−x^k−e_k^-=x_k-\hat x_k^-,把测量与预测读数的差称为新息:

yk=zk−Hkx^k−=Hkek−+εk,Sk=HkPk−HkT+Rk.y_k=z_k-H_k\hat x_k^-=H_ke_k^-+\varepsilon_k,\qquad S_k=H_kP_k^-H_k^T+R_k.

SkS_k 是新息协方差,同时包含预测误差和传感器噪声。设修正量为 KkykK_ky_k,则剩余误差为 ek+=ek−−Kkyke_k^+=e_k^--K_ky_k。这里先把 KkK_k 视为待确定的矩阵。

最小均方误差的线性估计要求剩余误差与已利用的新息不相关。代入上式,并利用两种误差独立,可得:

0=E[(ek−−Kkyk)ykT]=Pk−HkT−KkSk.0=\mathbb E[(e_k^--K_ky_k)y_k^T] =P_k^-H_k^T-K_kS_k.

解出增益后,均值更新就是:

Kk=Pk−HkTSk−1,x^k+=x^k−+Kkyk.K_k=P_k^-H_k^TS_k^{-1},\qquad \hat x_k^+=\hat x_k^-+K_ky_k.

本例状态有 2 个分量、测量有 1 个分量,因此 KkK_k 为 2×12\times1,把位置空间中的新息映射成位置和速度的修正。在上述高斯条件下,剩余误差与新息不相关也就意味着独立,所以这个估计也是条件均值。genui{"citation":{"ref":"turn2view2"}}

从剩余误差推导协方差

将测量模型代入后验误差:

ek+=(I−KkHk)ek−−Kkεk.e_k^+=(I-K_kH_k)e_k^--K_k\varepsilon_k.

两项分别来自尚未消除的先验误差和本次引入的测量噪声。计算 E[ek+(ek+)T]\mathbb E[e_k^+(e_k^+)^T] 时,交叉项因独立且零均值而消失,得到 Joseph 形式:

Pk+=(I−KkHk)Pk−(I−KkHk)T+KkRkKkT.P_k^+=(I-K_kH_k)P_k^-(I-K_kH_k)^T+K_kR_kK_k^T.

II 是与状态维度相同的单位矩阵,本例为 2×22\times2。如果漏掉第二项,就等于忽略观测带来的噪声,通常会低估不确定性。

展开 Joseph 形式,利用 Sk=HkPk−HkT+RkS_k=H_kP_k^-H_k^T+R_k,得到:

Pk+=Pk−−KkHkPk−−Pk−HkTKkT+KkSkKkT.P_k^+=P_k^- -K_kH_kP_k^- -P_k^-H_k^TK_k^T+K_kS_kK_k^T.

采用前面求出的最优增益时,KkSk=Pk−HkTK_kS_k=P_k^-H_k^T,于是精确算术下可以简化为:

Pk+=(I−KkHk)Pk−=Pk−−KkSkKkT.P_k^+=(I-K_kH_k)P_k^-=P_k^- -K_kS_kK_k^T.

若人为调整增益,不能继续套用这个简式。代码采用 Joseph 形式:它把两项半正定矩阵相加,通常比相减简式更能抵抗浮点误差,但仍应检查数值结果。genui{"citation":{"ref":"turn1view0"}}

由于 KkSkKkTK_kS_kK_k^T 半正定,同一时刻更新后的不确定性不会在任何线性方向上增加;不一定每个方向都严格减小,也不保证某一次估计必然更接近真实状态。

算清一次位置与速度更新

采样间隔为 1 秒,没有控制输入。上一拍后验为 x^k−1+=[0,1]T\hat x_{k-1}^+=[0,1]^T、Pk−1+=diag⁡(1,1)P_{k-1}^+=\operatorname{diag}(1,1),采用匀速转移矩阵 FF 和过程噪声协方差 QQ:

F=[1101],Q=[0.2000.1].F=\begin{bmatrix}1&1\\0&1\end{bmatrix},\qquad Q=\begin{bmatrix}0.2&0\\0&0.1\end{bmatrix}.

QQ 是本例给定的离散状态过程噪声协方差,表示位置、速度分别受到独立扰动;它不是单一随机加速度模型离散化得到的矩阵。两个对角元素的单位分别为平方米、平方米/秒平方。

先完成预测:

x^k−=Fx^k−1+=[11],Pk−=FPk−1+FT+Q=[2.2111.1].\hat x_k^-=F\hat x_{k-1}^+=\begin{bmatrix}1\\1\end{bmatrix},\qquad P_k^-=FP_{k-1}^+F^T+Q=\begin{bmatrix}2.2&1\\1&1.1\end{bmatrix}.

取位置读数 zk=2.5z_k=2.5 米、测量方差 Rk=0.8R_k=0.8 平方米,可得:

yk=1.5,Sk=2.2+0.8=3,Kk=[2.2/31/3].y_k=1.5,\qquad S_k=2.2+0.8=3,\qquad K_k=\begin{bmatrix}2.2/3\\1/3\end{bmatrix}.

因此位置修正 1.11.1 米、速度修正 0.50.5 米/秒,结果如下:

检查量 更新前 更新后
位置均值(米) 1 2.1
速度均值(米/秒) 1 1.5
位置方差(平方米) 2.2 0.586667
速度方差(平方米/秒平方) 1.1 0.766667
位置与速度误差协方差(平方米/秒) 1 0.266667

速度能被修正,是因为先验位置与速度误差的交叉协方差为 1,速度增益为 1/31/3。传感器并没有直接测量速度;如果这一刻交叉协方差为零,这次位置更新就不会改变速度均值。

同一份先验分别参与均值与协方差修正,后验进入下一拍预测

运行完整循环并检查边界

在已安装 NumPy 的 Python 3 环境中运行以下完整脚本。状态和测量用一维数组保存,FF、PP、HH、RR 保持二维,避免列向量与一维数组混用引发广播错误。np.linalg.solve 解线性方程,不显式计算逆矩阵。genui{"citation":{"ref":"turn2view1"}}

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

预期输出:

text
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 米时,新息变为零,后验均值仍为 [1,1]T[1,1]^T,但后验协方差保持表中结果。这是因为固定 Pk−P_k^-、HkH_k、RkR_k 后,协方差公式不含读数;新观测与预测一致,仍提供了信息。

若这拍没有有效读数,应跳过测量更新,令后验等于先验;不要伪造零新息再缩小协方差。同一条观测也不能重复作为独立证据更新,否则会过度自信。下一拍从本拍后验重新预测,加入 QQ 后不确定性仍可能增大。

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

目录与阅读设置

阅读设置

17

记录一个想法

后验更新怎样修正均值和协方差

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

登录哪都通