自动控制 / 系统课时

用新息衡量预测与测量的偏差

用新息衡量预测与测量的偏差

预测位置是10米,传感器却读到13米,这3米差距是否值得警惕?本篇先把状态预测映射成传感器读数,再推导新息及其协方差,用一组位置、速度数据说明:偏差的大小必须结合预测和测量各自的不确定性解释。

学习目标

  • 正确计算二维位置、速度状态对应的一维位置新息,明确做差顺序。
  • 从预测误差与测量噪声推导新息协方差,区分方差与标准差。
  • 用 NumPy 计算标准化新息,解释3米偏差在给定模型下的统计意义。

在测量空间计算偏差

设本拍真实状态为 xk=[pk,vk]Tx_k=[p_k,v_k]^{\mathsf T},包含位置和速度。x^k−\hat x_k^- 表示仅使用此前数据得到的本拍先验估计;上标减号表示“尚未用本拍测量更新”,不是负数。线性观测模型及预测观测为:

zk=Hkxk+ϵk,z^k=Hkx^k−z_k=H_kx_k+\epsilon_k,\qquad \hat z_k=H_k\hat x_k^-

其中 ϵk\epsilon_k 是本拍测量噪声。新息又叫更新前残差,需要先拿到本拍测量,再用它减去尚未更新的预测观测:

νk=zk−z^k=zk−Hkx^k−\nu_k=z_k-\hat z_k=z_k-H_k\hat x_k^-

若状态有 nn 个分量、测量有 mm 个分量,则 HkH_k 为 m×nm\times n 矩阵,新息是 mm 维向量。本例 n=2n=2、m=1m=1,传感器只测位置,所以不能把一个位置读数直接减去包含位置、速度的整个状态。

取预测位置10米、预测速度2米/秒,Hk=[1  0]H_k=[1\;0],实测位置13米。预测观测为10米,新息为 13−10=313-10=3 米。正号只说明测量大于预测,不能据此断定真实位置高了3米,也不能判定传感器存在正偏差。

从两种误差推导新息协方差

令先验估计误差 ek−=xk−x^k−e_k^-=x_k-\hat x_k^-,其协方差为 Pk−P_k^-。以下统计量均以此前测量为条件,省略条件记号。假设测量噪声均值为零、协方差为 RkR_k,并与先验误差不相关。代入观测模型可得:

νk=Hkek−+ϵk\nu_k=H_ke_k^-+\epsilon_k

新息同时包含状态预测误差在测量空间中的投影,以及本拍传感器噪声。对上式取协方差;线性映射产生 HkPk−HkTH_kP_k^-H_k^{\mathsf T},两个交叉协方差项因不相关而为零,因此:

Sk=Cov⁡(νk)=HkPk−HkT+RkS_k=\operatorname{Cov}(\nu_k)=H_kP_k^-H_k^{\mathsf T}+R_k

SkS_k 是 m×mm\times m 的测量空间协方差,通常不等于状态协方差 Pk−P_k^-,也不等于传感器噪声协方差 RkR_k。genui{"citation":{"ref":"turn2view0"}}

延续上面的读数,给定以下协方差数值;状态分量依次采用米和米/秒作为单位:

Pk−=[4111],Rk=[9]P_k^-=\begin{bmatrix}4&1\\1&1\end{bmatrix},\qquad R_k=[9]

其中位置方差为4平方米,速度方差为 1 m2/s21\,\mathrm{m^2/s^2},位置与速度的互协方差为 1 m2/s1\,\mathrm{m^2/s}。传感器位置噪声标准差为3米,因此 Rk=32=9R_k=3^2=9 平方米;不能直接把3当作方差。

由于 HkH_k 只选位置,HkPk−HkT=4H_kP_k^-H_k^{\mathsf T}=4 平方米,得到 Sk=4+9=13S_k=4+9=13 平方米。SkS_k 不是本次新息的平方 32=93^2=9;它表示模型预期的偏差散布,只改变这一次读数不会改变本例的 SkS_k。

状态预测映射后与实际读数做差,两类不确定性在测量空间相加

用 NumPy 复算同一例子

在已安装 NumPy 的 Python 3 环境中运行下面的完整脚本。x_pred 和 z 使用一维数组表示向量,H、P_pred、R 保持二维;本例新息形状必须为 (1,),S 的形状必须为 (1, 1)。

除新息和 SS 外,脚本还计算标量标准化新息 u=ν/Su=\nu/\sqrt S,以及归一化新息平方 NIS=νTS−1ν\mathrm{NIS}=\nu^{\mathsf T}S^{-1}\nu。二者均无量纲;标量情况下 NIS=u2\mathrm{NIS}=u^2。

python
import numpy as np

# 状态顺序固定为位置、速度;协方差使用本拍更新前的值。
x_pred = np.array([10.0, 2.0])
P_pred = np.array([[4.0, 1.0], [1.0, 1.0]])
H = np.array([[1.0, 0.0]])
R = np.array([[9.0]])
z = np.array([13.0])

# 先映射到测量空间,再计算偏差和对应的不确定性。
z_pred = H @ x_pred
innovation = z - z_pred
S = H @ P_pred @ H.T + R

# 本例只有一个位置读数,标准差可直接由唯一的方差开方。
sigma = np.sqrt(S[0, 0])
normalized = innovation[0] / sigma
# 解线性方程计算二次型,不显式构造 S 的逆矩阵。
nis = float(innovation @ np.linalg.solve(S, innovation))

print(f"predicted_position = {z_pred[0]:.3f} m")
print(f"innovation = {innovation[0]:.3f} m")
print(f"S = {S[0, 0]:.3f} m^2")
print(f"innovation_std = {sigma:.3f} m")
print(f"normalized_innovation = {normalized:.3f}")
print(f"NIS = {nis:.3f}")

预期输出:

text
predicted_position = 10.000 m
innovation = 3.000 m
S = 13.000 m^2
innovation_std = 3.606 m
normalized_innovation = 0.832
NIS = 0.692

np.linalg.solve(S, innovation) 求解 Sw=νSw=\nu,再用 νTw\nu^{\mathsf T}w 得到 NIS;该接口要求系数矩阵是满秩方阵。不要把矩阵乘法 @ 改成逐元素乘法 *。genui{"citation":{"ref":"turn2view1"}}

怎样解释这一次偏差

在线性高斯模型、先验分布和噪声设定都正确时,给定此前测量,新息服从 N(0,Sk)\mathcal N(0,S_k)。零均值描述重复试验的统计性质,不要求每一拍新息都等于零。本例3米只相当于约0.832个标准差,与所设的不确定性尺度相容。

若另一组已正确标定的条件给出 S=0.25S=0.25 平方米,同样的3米偏差就达到6个标准差。因此,固定使用“超过3米就是异常”的规则,无法适应不同传感器精度或不同预测不确定性。

多维测量应使用完整 SS 计算 NIS。逐分量除以各自标准差只能调整尺度,不能消除测量之间的相关性。若 SS 奇异,应检查是否重复使用同一读数却错误处理相关性,以及协方差和矩阵构造。有效的 P−P^- 半正定且 RR 正定时,SS 应当正定。

实际记录时,先保存本拍的 νk\nu_k 和 SkS_k,再进行状态更新。更新后的 zk−Hkx^k+z_k-H_k\hat x_k^+ 是另一种残差,其中 x^k+\hat x_k^+ 已经使用了同一测量;不能用它替换新息来评价原始预测。

可以先记录连续100帧的标准化新息:样本均值持续偏离零时,优先核对传感器偏置、时间戳和观测模型;标准化新息或 NIS 频繁偏大时,再检查运动模型及噪声参数。

100帧只是记录窗口示例,有限样本的均值不必恰好为零,单次 NIS 小也不能证明模型正确。这些现象是排查线索;由于新息还包含预测误差,不应直接把它的样本方差当作 RR。

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

目录与阅读设置

阅读设置

17

记录一个想法

用新息衡量预测与测量的偏差

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

登录哪都通