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

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

运动规律

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

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

    • 已知初始条件

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

    计算预测值

    1. 预测位置

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

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

    2. 预测速度

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

    预测误差协方差矩阵 P_{k|k-1}

    误差协方差矩阵的计算公式为：
    P_{k|k-1} = A P_{k-1|k-1} A^T + Q

    已知条件：

  • 状态转移矩阵 A：
    A = [1, 1] [0, 1]

  • 初始误差协方差矩阵 P_{0|0}：
    P_{0|0} = [10, 0] [ 0, 5]

  • 过程噪声协方差 Q：
    Q = [1, 0] [0, 1]

    • 计算步骤：

  • 计算 A P_{0|0}：
    A P_{0|0} = [1, 1] [10, 0] = [10, 5] [0, 1] x [ 0, 5] [ 0, 5]

  • 计算 A P_{0|0} A^T：
    A P_{0|0} A^T = [10, 5] [1, 0] = [15, 5] [ 0, 5] x [1, 1] [ 5, 6]

  • 加上 Q：
    P_{k|k-1} = [15, 5] [ 5, 6] + [1, 0] = [16, 5] [0, 1] [ 5, 7]

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

    假设感测器测量到的位置

    感测器测量值：
    z_1 = 22 公尺

    测量可能有误差，感测器的不确定性用测量误差协方差 R 表示。

    已知条件：

  • 测量误差协方差 R：
    R = 4

  • 观测矩阵 H：
    H = [1, 0]

    • 卡尔曼增益的计算

    卡尔曼增益公式：
    K_k = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1}

    步骤：

  • 计算 H P_{k|k-1}：
    H P_{k|k-1} = [1, 0] [16, 5] = [16, 5] [ 5, 7]

  • 计算 H P_{k|k-1} H^T：
    H P_{k|k-1} H^T = [16, 5] [1] = 16 [0]

  • 计算分母 H P_{k|k-1} H^T + R：
    H P_{k|k-1} H^T + R = 16 + 4 = 20

  • 计算 K_k：
    K_k = [16, 5] (1/20) = [0.8, 0.25]

    • 更新状态

  • 测量差异：
    差异 = z_1 - x_{1|0} = 22 - 20 = 2 公尺

  • 修正位置：
    修正公式：
    x_{1|1} = x_{1|0} + (K_{k,1} × 差异)

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

  • 修正速度：
    修正公式：
    v_{1|1} = v_{1|0} + (K_{k,2} × 差异)

    • 带入数据：
    v_{1|1} = 20 + (0.25 × 2) = 20 + 0.5 = 20.5 公尺/秒

    结果

  • 更新后的状态：
    • 位置：21.6 公尺
    • 速度：20.5 公尺/秒
  • 卡尔曼增益的解释：
    • K_{k,1} = 0.8：位置修正权重。
    • K_{k,2} = 0.25：速度修正权重。
  • 更新后的误差协方差矩阵 P_{k|k}：
    P_{k|k} = [3.2, 1.25] [1.25, 4.75]

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

  • 预测位置和速度：
    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 公尺

    • 重复上述修正步骤。