卡爾曼濾波器例子：追蹤汽車的位置與速度

第一步：預測（依據運動規律）

運動規律

假設汽車的運動規律如下：

1. 位置變化：
   每秒的「位置」 = 前一秒的位置 + 速度 × 時間。
2. 速度保持不變。

已知初始條件

• 初始位置：
  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

已知條件：

1. 狀態轉移矩陣 A：
   A = [1, 1] [0, 1]

2. 初始誤差協方差矩陣 P_{0|0}：
   P_{0|0} = [10, 0] [ 0, 5]

3. 過程噪聲協方差 Q：
   Q = [1, 0] [0, 1]

計算步驟：

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

2. 計算 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]

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

第二步：更新（依據測量數據修正）

假設感測器測量到的位置

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

測量可能有誤差，感測器的不確定性用測量誤差協方差 R 表示。

已知條件：

1. 測量誤差協方差 R：
   R = 4

2. 觀測矩陣 H：
   H = [1, 0]

卡爾曼增益的計算

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

步驟：

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

2. 計算 H P_{k|k-1} H^T：
   H P_{k|k-1} H^T = [16, 5] [1] = 16 [0]

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

4. 計算 K_k：
   K_k = [16, 5] (1/20) = [0.8, 0.25]

更新狀態

1. 測量差異：
   差異 = z_1 - x_{1|0} = 22 - 20 = 2 公尺

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

帶入數據：
x_{1|1} = 20 + (0.8 × 2) = 20 + 1.6 = 21.6 公尺

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

帶入數據：
v_{1|1} = 20 + (0.25 × 2) = 20 + 0.5 = 20.5 公尺/秒

結果

1. 更新後的狀態：

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

2. 卡爾曼增益的解釋：

• K_{k,1} = 0.8：位置修正權重。
• K_{k,2} = 0.25：速度修正權重。

3. 更新後的誤差協方差矩陣 P_{k|k}：
   P_{k|k} = [3.2, 1.25] [1.25, 4.75]

下一步：進行第二秒的預測與更新

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

2. 用新的測量值進行修正：
   假設下一秒測量值為：
   z_2 = 43 公尺

重複上述修正步驟。