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

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

運動規律

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

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 公尺/秒。

預測結果

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

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

假設感測器測量到的位置

感測器測量到的位置為：
z_1 = 22 公尺
但測量可能有誤差。

測量與修正

1. 測量差異：

   測量值與預測值的差異：
   差異 = z_1 - x_1

   帶入數據：
   差異 = 22 - 20 = 2 公尺。

2. 卡爾曼增益（修正權重）：

   • 預測的不確定性：我們對預測值的信任程度。
   • 測量的不確定性：感測器的測量準確度。

   假設計算卡爾曼增益得到：
   卡爾曼增益 = 0.8

3. 修正位置：

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

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

4. 修正速度：

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

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

第三步：結果

1. 更新後的狀態：

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

2. 更新後的誤差：
   修正後，對位置和速度的估計誤差減少，結果更可信。

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

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 公尺

   重複上述修正步驟。