自动控制 / 系统课时

只用位置观测估计隐藏的速度

只用位置观测估计隐藏的速度

位置传感器每隔0.1秒只给一个带噪读数,速度却需要连续估计。这里把位置和速度组成二维状态,用完整的预测、更新循环处理201个样本,再用曲线和误差指标检查:没有速度传感器,速度估计是否仍能接近真值。

学习目标

  • 配置二维状态与一维位置观测,说明速度修正量来自哪里。
  • 运行完整的 NumPy 滤波循环,绘制位置与速度估计。
  • 在相同时间窗内比较测量、差分和滤波误差,检查初值与采样时序。

先把观测与真值分开

仿真目标从2 m出发,以1.5 m/s匀速运动,在0~20 s内每0.1 s采样一次。位置读数加入相互独立、均值为0、标准差为0.8 m的高斯噪声,所以测量方差为 R=0.64 m2R=0.64\ \mathrm{m^2}。真实速度只用于验收,滤波器的外部观测只有位置。

状态顺序固定为位置、速度,观测矩阵 H 取 [[1.0, 0.0]]。匀速模型在预测时把速度乘以采样间隔,累加到位置;预测速度保持原值,但随后的测量更新仍可以改变它。

直接用相邻读数计算速度,相当于把两次测量误差之差除以0.1 s。在独立噪声假设下,差分速度的噪声标准差为 2×0.8/0.1≈11.31 m/s\sqrt{2}\times0.8/0.1\approx11.31\ \mathrm{m/s};每拍真实位移仅0.15 m,因此差分曲线很容易被噪声淹没。

位置新息怎样改变速度

预测同时传播状态均值和协方差。速度误差会通过位移项影响位置误差,使位置与速度的互协方差非零。上标 −- 表示本拍更新前的预测,++ 表示吸收本拍读数后的估计;位置新息及速度修正满足:

rk=zk−p^k−Kv,k=Pvp,k−Ppp,k−+Rv^k+=v^k−+Kv,krk\begin{aligned} r_k&=z_k-\hat p_k^-\\ K_{v,k}&=\frac{P_{vp,k}^-}{P_{pp,k}^-+R}\\ \hat v_k^+&=\hat v_k^-+K_{v,k}r_k \end{aligned}

这里 Ppp−P_{pp}^- 是预测位置方差,Pvp−P_{vp}^- 是速度与位置的预测误差互协方差。位置新息只有一个,却可以经两个增益分别修正位置和速度;速度增益的单位是每秒。

用一拍独立算例检查:预测位置5 m、速度1.2 m/s,位置方差1 m²、互协方差0.4 m²/s,读数5.5 m,测量方差0.64 m²。此时速度增益约0.244 s⁻¹,速度增加约0.122 m/s,得到约1.322 m/s。

同一位置新息经两路增益修正位置和速度,速度支路依赖互协方差

把预测、更新和绘图连起来

下面代码已在 Python 3.12、NumPy 2.3.5、Matplotlib 3.10.8 下验证。将它整体放进已安装这两个库的 Jupyter 单元中运行。状态 x 的形状为 (2,),P、F、Q 为 (2, 2),H 为 (1, 2),R 为 (1, 1)。

初始位置和速度估计都取0,初始标准差分别取2 m和3 m/s,因此协方差对角线填4和9。初速取0只是粗略猜测,并不表示已经确定目标静止。

过程噪声采用每个采样区间内恒定、区间之间独立的随机加速度假设,标准差取0.2 m/s²,用 Q=σa2GGTQ=\sigma_a^2GG^{\mathsf T} 构造协方差。这对应 FilterPy 文档中的离散噪声模型。本例真值严格匀速,非零 Q 是估计器容许模型偏差的余量,生成真值时没有加入该过程扰动。genui{"citation":{"ref":"turn2view1"}}

update 通过 np.linalg.solve 求解增益,并用 Joseph 形式更新协方差,最后消除浮点运算造成的微小非对称。每条位置读数只更新一次,第一条读数之前不做预测。genui{"citation":{"refs":["turn1view0","turn2view2"]}}

python
import numpy as np
import matplotlib.pyplot as plt

def predict(x, P, F, Q):
    # 均值和误差协方差必须一起传播。
    return F @ x, F @ P @ F.T + Q

def update(x, P, z, H, R):
    innovation = np.array([z]) - H @ x
    S = H @ P @ H.T + R
    K = np.linalg.solve(S, H @ P).T
    x = x + K @ innovation
    # Joseph 形式使用本拍更新前的 P,减轻舍入误差的影响。
    A = np.eye(x.size) - K @ H
    P = A @ P @ A.T + K @ R @ K.T
    return x, 0.5 * (P + P.T)

def rmse(error):
    return float(np.sqrt(np.mean(error ** 2)))

def main():
    dt = 0.1
    t = np.arange(201) * dt
    rng = np.random.default_rng(7)
    sigma_z, sigma_a = 0.8, 0.2
    # 真值只用于生成传感器读数、计算误差和画参考线。
    p_true = 2.0 + 1.5 * t
    v_true = np.full(t.size, 1.5)
    z = p_true + rng.normal(0.0, sigma_z, t.size)

    F = np.array([[1.0, dt], [0.0, 1.0]])
    H = np.array([[1.0, 0.0]])
    G = np.array([0.5 * dt ** 2, dt])
    Q = sigma_a ** 2 * np.outer(G, G)
    R = np.array([[sigma_z ** 2]])
    x = np.array([0.0, 0.0])
    P = np.diag([2.0 ** 2, 3.0 ** 2])
    estimates = np.empty((t.size, 2))

    for k, measurement in enumerate(z):
        # 初值位于 t=0;第一条读数只更新,避免提前预测一拍。
        if k > 0:
            x, P = predict(x, P, F, Q)
        x, P = update(x, P, measurement, H, R)
        estimates[k] = x

    # 差分仅作对照,绝不作为第二路观测输入滤波器。
    v_diff = np.diff(z) / dt
    use = t >= 2.0
    use_diff = t[1:] >= 2.0
    raw_p = rmse(z[use] - p_true[use])
    kf_p = rmse(estimates[use, 0] - p_true[use])
    raw_v = rmse(v_diff[use_diff] - v_true[1:][use_diff])
    kf_v = rmse(estimates[use, 1] - v_true[use])
    print(f'position RMSE: raw={raw_p:.3f}, KF={kf_p:.3f} m')
    print(f'velocity RMSE: diff={raw_v:.3f}, KF={kf_v:.3f} m/s')
    print(f'last estimate: p={x[0]:.3f} m, v={x[1]:.3f} m/s')

    fig, axes = plt.subplots(2, 1, sharex=True, figsize=(10, 6))
    axes[0].scatter(t, z, s=9, color='#d9822b',
                    alpha=0.55, label='Position measurements')
    axes[0].plot(t, p_true, '--', color='#334155', label='True position')
    axes[0].plot(t, estimates[:, 0], color='#149d91', label='KF position')
    axes[0].set_ylabel('Position (m)')
    axes[1].plot(t, v_true, '--', color='#334155', label='True velocity')
    axes[1].plot(t, estimates[:, 1], color='#149d91', label='KF velocity')
    axes[1].set_ylabel('Velocity (m/s)')
    axes[1].set_xlabel('Time (s)')
    for ax in axes:
        ax.axvline(2.0, color='gray', linestyle=':', linewidth=0.8)
        ax.grid(alpha=0.25)
        ax.legend(loc='best')
    fig.tight_layout()
    plt.show()

if __name__ == '__main__':
    main()

预期输出(随机种子为7):

text
position RMSE: raw=0.713, KF=0.256 m
velocity RMSE: diff=10.459, KF=0.092 m/s
last estimate: p=32.172 m, v=1.591 m/s

同时显示上下两个子图:上图比较橙色位置读数、深蓝灰虚线真值和青绿色滤波位置;下图比较速度真值与滤波速度。图中的竖虚线标记2 s,图例采用英文以避免依赖额外中文字体。

用相同时间窗验收

输出的误差均取2~20 s,明确排除启动阶段。位置比较使用相同的时间索引;差分速度对应 t[1:],也筛选到相同时间窗。RMSE 是误差平方取平均后再开方,单位与被评估的量相同。

这次运行的位置 RMSE 从0.713 m降至0.256 m,速度 RMSE 从10.459 m/s降至0.092 m/s。末时刻真值是32 m和1.5 m/s,估计仍有小幅误差;指标改善不表示每个采样点都更准确,也不保证更换数据后仍有相同比例的改善。

第一帧速度仍是0属于正常现象:初始 P 为对角阵,尚无位置与速度的误差关联,所以第一次位置更新的速度增益为0。后续预测产生互协方差,位置新息才开始修正速度。不要为了让曲线开头好看,把真实速度填进初值。

虽然仿真预先生成了整段数据,循环在时刻 kk 只使用第0至第 kk 条读数,仍属于在线滤波。状态维度固定时,处理 NN 个样本的时间复杂度为 O(N)O(N);只保留当前状态需要 O(1)O(1) 空间,本例为绘图保存历史则需要 O(N)O(N) 空间。

速度不动或响应迟缓时检查什么

  1. 检查 F[0, 1] 是否等于0.1。误设为0会破坏匀速模型中速度对位置的影响,不能靠修改绘图来修复。
  2. 检查 P[1, 1] 是否把未知速度误设成完全确定。本例初值为9;若速度方差、互协方差和速度过程噪声都为0,速度增益会一直为0。
  3. 检查循环是否保留上一拍的 x、P,并在更新后记录结果。每轮重新初始化会丢掉历史信息;把差分速度当成独立观测再次输入,还会重复使用同一批位置噪声。

如果目标持续加速,匀速模型可能出现滞后,需要重新评估加速度噪声或增加加速度状态。真实采样间隔不固定时,每拍都应根据实际时间差重建 F 和 Q,不能继续沿用固定的0.1 s。

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

目录与阅读设置

阅读设置

17

记录一个想法

只用位置观测估计隐藏的速度

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

登录哪都通