自动控制 / 系统课时

如何区分过程噪声 Q 与测量噪声 R

如何区分过程噪声 Q 与测量噪声 R

位置读数抖动,可能来自小车真实运动的变化,也可能来自传感器误差。本文用位置与速度两维状态,分别构造过程噪声协方差 Q 和测量噪声协方差 R,并通过可运行算例检查矩阵是否合法、怎样影响测量权重。

学习目标

  • 根据噪声进入的位置,区分真实运动扰动与测量读数误差。
  • 为两维位置速度状态构造 2×22\times2 的 Q,为单个位置测量构造 1×11\times1 的 R。
  • 检查协方差的半正定性,并用数值增益解释改变 Q、R 后的响应。

按噪声进入的位置判断来源

设小车状态为 xk=[pk,vk]Tx_k=[p_k,v_k]^T,传感器只测位置。随机推力使小车偏离匀速预测,属于过程扰动;小车保持静止而读数上下跳动,属于测量噪声。两者进入不同方程:

xk=Fxk−1+wk−1zk=Hxk+εk\begin{aligned} x_k&=Fx_{k-1}+w_{k-1}\\ z_k&=Hx_k+\varepsilon_k \end{aligned}

这里 F 是匀速转移矩阵,H=[1,0]H=[1,0]。Q=Cov⁡(wk−1)Q=\operatorname{Cov}(w_{k-1}) 描述每一步新增的状态扰动,R=Cov⁡(εk)R=\operatorname{Cov}(\varepsilon_k) 描述读数误差。Q、R 是协方差,不是某次随机噪声的取值。

过程扰动在状态传播时进入,测量噪声在生成读数时进入

以下采用零均值、不同采样时刻之间不相关、两类噪声彼此不相关,并与初始估计误差不相关的模型。已知控制加速度应放进确定性输入项;持续的传感器偏置应校准或另建偏置状态。

从静止读数估计 R

将目标固定在 2 m,取三个教学读数 1.5、2.0、2.5 m。样本均值为 2 m,按 N−1N-1 作分母计算样本方差,得到 0.25 m20.25\,\mathrm{m^2},对应样本标准差 0.5 m0.5\,\mathrm{m}。因此可用 R=[0.25]R=[0.25] 作为测量噪声方差的估计。

这里只有三个点,便于手算;实际标定可先采集 200 个静止读数,保持与运行时相同的采样率和传感器设置。若规格给出的是标准差 σz\sigma_z,应填 R=[σz2]R=[\sigma_z^2];标称“最大误差”不能直接当成标准差。

不要对运动过程中的原始位置直接求方差,否则真实位移也会被算进 R。固定的 +0.2 m+0.2\,\mathrm{m} 偏置同样不能靠增大 R 消除,应先检查读数均值与已知位置的差。

把随机加速度映射成 Q

取采样间隔 Δt=0.1 s\Delta t=0.1\,\mathrm{s}。假设每个区间内存在一个保持不变的随机加速度 a,各区间之间独立,均值为零,标准差为 σa=0.6 m/s2\sigma_a=0.6\,\mathrm{m/s^2}。它分别造成位置误差 aΔt2/2a\Delta t^2/2 和速度误差 aΔta\Delta t:

w=Ga,G=[Δt2/2Δt]w=Ga,\qquad G=\begin{bmatrix}\Delta t^2/2\\\Delta t\end{bmatrix}
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}

同一个加速度同时影响位置和速度,所以交叉协方差不为零;本例 Q12=Q21=0.00018 m2/sQ_{12}=Q_{21}=0.00018\,\mathrm{m^2/s}。把 Q 随意改成对角矩阵,会丢掉这个扰动模型规定的相关性。改变采样间隔时,也要重新计算 Q。

检查维度、单位和半正定性

对于 n 维状态、m 维观测,状态扰动协方差 Q 为 n×nn\times n,测量噪声协方差 R 为 m×mm\times m。本例虽估计位置和速度,却只测位置,所以 R 仍是 1×11\times1。

元素 本例含义 单位
Q11Q_{11} 一步位置扰动的方差 m2\mathrm{m^2}
Q22Q_{22} 一步速度扰动的方差 m2/s2\mathrm{m^2/s^2}
Q12=Q21Q_{12}=Q_{21} 位置与速度扰动的协方差 m2/s\mathrm{m^2/s}
R11R_{11} 位置测量误差的方差 m2\mathrm{m^2}

协方差必须对称且半正定,即任意向量 u 都满足 uTQu≥0u^TQu\ge0,R 同理。非对角元素可以为负,但不能任意填写;二维矩阵还须满足 Q122≤Q11Q22Q_{12}^2\le Q_{11}Q_{22}。

本例 Q 的特征值为 0 和 0.003609,秩为 1,仍然合法:两个误差分量来自同一个随机加速度。不要要求 Q 必须可逆。R 在理论上也可以半正定;常规增益计算要求可逆的是创新协方差 S。本例 R=[0.25]R=[0.25] 为正定,能保证半正定预测协方差下的 S 可逆。

运行算例比较测量权重

令 P 表示当前估计误差协方差,P−P^- 表示下一步预测误差协方差。Q 在预测时加入,R 在计算测量残差的不确定性 S 时加入;卡尔曼增益 K 决定用多大的测量残差修正估计:

P−=FPFT+QS=HP−HT+RK=P−HTS−1\begin{aligned} P^-&=FPF^T+Q\\ S&=HP^-H^T+R\\ K&=P^-H^TS^{-1} \end{aligned}

上述公式对应标准线性卡尔曼滤波的预测和更新计算。实际实现可通过解线性方程求 K,避免显式计算逆矩阵。

在已安装 NumPy 的 Python 3 环境运行以下完整脚本。每组均从相同的 P=diag⁡(0.04,0.25)P=\operatorname{diag}(0.04,0.25) 出发,分别代表当前位置标准差 0.2 m、速度标准差 0.5 m/s。P 描述当前估计已有的不确定性,Q 描述下一步新增的扰动。

python
import numpy as np

np.set_printoptions(precision=6, suppress=True)
dt, sigma_a = 0.1, 0.6
F = np.array([[1., dt], [0., 1.]])
H = np.array([[1., 0.]])

# 同一加速度扰动同时影响位置和速度,保留交叉协方差。
G = np.array([[0.5 * dt**2], [dt]])
Q = sigma_a**2 * (G @ G.T)

# 三个静止读数仅用于手算演示;ddof=1 使用样本方差。
z_static = np.array([1.5, 2.0, 2.5])
R = np.array([[np.var(z_static, ddof=1)]])
assert Q.shape == (2, 2) and R.shape == (1, 1)

# 该数值尺度下允许 1e-12 的舍入误差,不要求 Q 严格正定。
for C in (Q, R):
    assert np.allclose(C, C.T, rtol=0, atol=1e-12)
    assert np.linalg.eigvalsh(C).min() >= -1e-12

print('Q =')
print(Q)
print('R =')
print(R)
print('eig(Q) =', np.linalg.eigvalsh(Q))

# 每组从相同的当前估计协方差出发,只比较一次更新的权重。
P = np.diag([0.04, 0.25])
cases = [('base', 1., 1.), ('Q x100', 100., 1.), ('R x4', 1., 4.)]
for label, q_scale, r_scale in cases:
    P_pred = F @ P @ F.T + q_scale * Q
    S = H @ P_pred @ H.T + r_scale * R
    K = np.linalg.solve(S, H @ P_pred).T
    print(f'{label}: S={S.item():.6f}, K_p={K[0, 0]:.6f}, K_v={K[1, 0]:.6f}')

预期输出:

text
Q =
[[0.000009 0.00018 ]
 [0.00018  0.0036  ]]
R =
[[0.25]]
eig(Q) = [0.       0.003609]
base: S=0.292509, K_p=0.145325, K_v=0.086083
Q x100: S=0.293400, K_p=0.147921, K_v=0.146558
R x4: S=1.042509, K_p=0.040776, K_v=0.024153

若位置读数比预测位置高 1 m,基准组会把位置上调约 0.145 m、速度上调约 0.086 m/s。R 放大四倍后,位置只上调约 0.041 m,说明本次测量的影响减弱。

Q 放大一百倍后,位置增益变化较小,速度增益却从 0.086083 增至 0.146558。这只是放大差异的对照参数,并非推荐设置;不同状态分量的响应幅度不能一概而论。

实际跟踪中,Q 过小或 R 过大常使机动跟随迟缓;Q 过大或 R 过小则可能让估计更受读数抖动影响。先用静止数据估计 R,再用含匀速与加速片段的独立验证轨迹检查误差和跟随延迟,不能只凭曲线是否平滑选择参数。

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

目录与阅读设置

阅读设置

17

记录一个想法

如何区分过程噪声 Q 与测量噪声 R

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

登录哪都通