卡尔曼滤波器例子：追踪汽车的位置与速度

第一步：预测（依据运动规律）

运动规律

假设汽车的运动规律如下：

  • 位置变化：
    每秒的「位置」 = 前一秒的位置 + 速度 × 时间。
  • 速度保持不变。

    • 已知初始条件

    • 初始位置：
      x_0 = 0 公尺。
    • 初始速度：
      v_0 = 20 公尺/秒。
    • 时间间隔：
      Δt = 1 秒。

    计算预测值

  • 预测位置：

    根据运动规律：
    x_1 = x_0 + v_0 × Δt

    带入数据：
    x_1 = 0 + 20 × 1 = 20 公尺。

  • 预测速度：

    速度保持不变：
    v_1 = v_0 = 20 公尺/秒。

    • 预测结果

    • 位置：20 公尺
    • 速度：20 公尺/秒

    第二步：更新（依据测量数据修正）

    假设感测器测量到的位置

    感测器测量到的位置为：
    z_1 = 22 公尺
    但测量可能有误差。

    测量与修正

  • 测量差异：

    测量值与预测值的差异：
    差异 = z_1 - x_1

    带入数据：
    差异 = 22 - 20 = 2 公尺。

  • 卡尔曼增益（修正权重）：

    • 预测的不确定性：我们对预测值的信任程度。
    • 测量的不确定性：感测器的测量準确度。

    假设计算卡尔曼增益得到：
    卡尔曼增益 = 0.8

  • 修正位置：

    修正公式：
    x_{1|1} = x_{1|0} + (卡尔曼增益 × 差异)

    带入数据：
    x_{1|1} = 20 + (0.8 × 2) = 20 + 1.6 = 21.6 公尺。

  • 修正速度：

    修正公式：
    v_{1|1} = v_{1|0} + (速度修正权重 × 差异)

    假设速度修正权重为 0.25：
    v_{1|1} = 20 + (0.25 × 2) = 20 + 0.5 = 20.5 公尺/秒。

    • 第三步：结果

  • 更新后的状态：

    • 位置：21.6 公尺
    • 速度：20.5 公尺/秒

  • 更新后的误差：
    修正后，对位置和速度的估计误差减少，结果更可信。

    • 下一步：进行第二秒的预测与更新

  • 预测位置和速度：
    x_2 = x_{1|1} + v_{1|1} × 1 = 21.6 + 20.5 = 42.1 公尺
    v_2 = v_{1|1} = 20.5 公尺/秒

  • 用新的测量值进行修正：
    假设下一秒测量值为：
    z_2 = 43 公尺

    重复上述修正步骤。