05.3 Kalman 濾波#

本章導讀#

狀態回饋需要知道狀態，但真實感測器有雜訊。 Kalman 濾波是雜訊下的最佳狀態估計器：把模型預測與量測資料做最佳加權融合。 Apollo 登月、飛彈導引、手機導航——全靠它。

淺顯說明#

你在霧裡走路：

模型預測：「我往東走了 10 步，應該在這裡。」
量測：「指南針說我在偏北一點的位置。」（但指南針有誤差）

兩個資訊都不完全可靠，怎麼融合？誰更可靠就多信誰—— 模型誤差小就多信模型，量測誤差小就多信量測。 Kalman 濾波就是把「多信誰」算得嚴格且即時（遞迴更新，每步只存一個估計值）。

正規理論#

系統與雜訊模型#

離散時間系統（含過程雜訊 \(w\) 與量測雜訊 \(v\) ）：

\[ x_{k+1} = \Phi x_k + \Gamma u_k + w_k, \qquad y_k = H x_k + v_k \]

雜訊假設為高斯白雜訊：

\[ \mathbb{E}[w_k] = 0,\ \mathbb{E}[w_kw_k^T] = Q; \qquad \mathbb{E}[v_k] = 0,\ \mathbb{E}[v_kv_k^T] = R \]

Kalman 濾波器#

預測步（用模型猜下一刻）：

\[ \hat x_{k+1}^- = \Phi \hat x_k + \Gamma u_k, \qquad P_{k+1}^- = \Phi P_k \Phi^T + Q \]

更新步（用量測修正）：

\[ K_{k+1} = P_{k+1}^- H^T\left(H P_{k+1}^- H^T + R\right)^{-1} \]

\[ \hat x_{k+1} = \hat x_{k+1}^- + K_{k+1}\left(y_{k+1} - H\hat x_{k+1}^-\right), \qquad P_{k+1} = (I - K_{k+1}H)P_{k+1}^- \]

Kalman 增益的直觀#

增益 \(K\) 是「多信誰」的權重：

\(R\) 大（量測很不準） \(\Rightarrow K\) 小 \(\Rightarrow\) 多信模型預測。
\(Q\) 大（模型很不準） \(\Rightarrow P^-\) 大 \(\Rightarrow K\) 大 \(\Rightarrow\) 多信量測。

最佳性#

Kalman 濾波是最小變異數估計：在所有線性估計器中，它使估計誤差共變異矩陣 \(\mathbb{E}[(x - \hat x)(x - \hat x)^T]\) 最小（跡最小）。高斯情形下，它也是所有估計器（含非線性）中的條件平均 \(\mathbb{E}[x_k \mid y_1,\dots,y_k]\) 。

簡易範例：一維位置估計#

靜止目標（ \(\Phi = 1\) ， \(Q = 0.01\) ，量測雜訊 \(R = 1\) ），量測值 \(y = 5, 6, 4.5\) ：

初始 \(\hat x_0 = 0\) ， \(P_0 = 1\) 。
第 1 步： \(P^- = 1 + 0.01 = 1.01\) ， \(K = \dfrac{1.01}{1.01 + 1} \approx 0.502\) ， \(\hat x_1 = 0 + 0.502 \times 5 \approx 2.51\) ， \(P_1 = (1 - 0.502)(1.01) \approx 0.503\) 。
第 2 步： \(K = \dfrac{0.513}{1.513} \approx 0.339\) ， \(\hat x_2 = 2.51 + 0.339(6 - 2.51) \approx 3.69\) 。
第 3 步： \(K \approx 0.34\) ， \(\hat x_3 \approx 3.69 + 0.34(4.5 - 3.69) \approx 3.97\) 。

估計值收斂到 \(4\) – \(5\) 之間，且 \(P\) 逐步縮小（越來越有信心）。 每一步只加權平均一次——這就是遞迴的力量：不需要儲存全部歷史資料。

歷史典故#

1960 年 Kalman 發表濾波理論，見 1960-Kalman狀態空間與濾波.md—— 隨後數年內被用於 Apollo 軌道估計。
1948 年 Wiener 的《控制論》含早期的 Wiener 濾波（頻域、非遞迴）， Kalman 濾波是其時域遞迴後繼者，見 1948-Wiener控制論.md。

習題#

上例中，若 \(R = 0.01\) （量測很準），第 1 步的 \(K\) 會是多少？說明直觀。
證明： \(\Phi = 1, Q = 0\) 時， \(P_k \to 0\) 且 \(\hat x_k\) 收斂到量測的平均值。