卡爾曼濾波

卡爾曼濾波法

卡爾曼濾波算法是一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的算法，是一種最優化自回歸數據處理算法。

通俗地講，對系統 \(k-1\) 時刻的狀態，我們有兩種途徑來獲得系統 \(k\) 時刻的狀態。一種是根據常識或者系統以往的狀態表現來預測 \(k\) 時刻的狀態，這個量我們稱之為預測量；另一種是通過傳感器等進行系統 \(k\) 時刻我們所觀測變量狀態的測量，這個量我們稱之為觀測量。

顯然，兩種途徑均有誤差，而卡爾曼濾波要做的事，就是結合這兩個結果，濾去兩種結果中的“噪聲”，得到一個更加准確的系統 \(k\) 時刻的狀態的估計。下面就來看卡爾曼濾波算法具體是怎么做到的。

系統狀態預測方程

\[X(k) = AX(k-1) + BU(k) + W(k-1) \]

系統狀態觀測方程

\[Z(k) = HX(k) + V(k) \]

卡爾曼濾波的計算過程

卡爾曼濾波向前的推進包含預測和觀測過程。

下面的計算式中用 \(\hat x_k^-\) 表示 \(k\) 時刻的預測值， \(z_k\) 表示 \(k\) 時刻的觀測值，\(\hat x_k\) 表示 \(k\) 時刻的卡爾曼估計值, \(x_k\) 表示系統 \(k\) 時刻的真實狀態值。

時間更新（預測）

1. 根據上述的系統狀態預測方程計算出 \(k\) 時刻的系統狀態

   \[\hat{x}_{k}^- = A\hat x_{k-1} + Bu_{k-1} \]

2. 計算 \(k\) 時刻的誤差的協方差

   \[P_k^- = AP_{k-1}A^T + Q \]

測量更新（校正）

3. 計算卡爾曼增益

   \[K_k = \frac{P_k^- H^T}{HP_k^-H^T + R} \]

   上面的 \(P_k^-\) 即為 \(k\) 時刻的預測誤差, \(R\) 為 \(k\) 時刻的系統觀測誤差。

4. 校正預測得到的 \(k\) 時刻的系統狀態

   \[\hat x_k = \hat x_k^- + K_k(z_k -H\hat x_{k}^-) \]

5. 更新測量誤差的協方差

   \[P_k = (I-K_kH)P_k^- \]

   \(I\) 為單位矩陣。

卡爾曼增益推導

顯然，卡爾曼濾波作為一種數據融合算法，其核心在於觀測量和預測量的比例取值，也就是卡爾曼增益。

由於推導的數學過程比較復雜， 留着以后推導。

我們的主要目的是盡量縮小卡爾曼估計值 \(\hat x_k\) 與真實值 \(x_k\) 的誤差大小，可推導出兩者的差值為：

\[\begin{aligned} x_k-\hat x_{{k}} &= x_k - (\hat x_{k}^- + K_k(z_k - H\hat x_{k}))\\ &= x_k - \hat x_k^- - K_k(Hx_k + v_k) + K_kH\hat x_k\\ &= (1-K_kH)x_k - (I-K_kH)\hat x_k -K_kv_k\\ &= (I-K_kH)(x_k - \hat x_k^-) - K_k v_k \end{aligned} \]

令 \(e_k = x_k-\hat x_k\)，由於 \(x_k-\hat x_k^-\) 和 \(v_k\) 均可以視作服從高斯分布的噪聲，所以 \(e_k\) 總體服從高斯分布， \(e_k ～ N(0, P_k)\)

易知，要使卡爾曼估計值盡可能接近真實值，\(e_k\) 的方差應該盡可能小。

因此求卡爾曼增益的問題轉化成了求

\[P_k = f(K_k) \]

取最小值時 \(K_k\) 的取值。

由於均值 \(\mu = 0\)，根據方差的定義， \(P_k = E(e_ke_k^T)\)。

接下來就是將 \(e_k = (I - K_kH)(x_k-\hat x_k^-) - K_kv_k\) 帶入上式展開，比較復雜，直接寫結果：

我們用 \(\hat e_k\) 表示預測值與真實值的誤差，稱為估計誤差。

則有

\[\begin{aligned} P_k = E\{[(I&-K_kH)\hat e_k\hat e_k^T(1-K_kH)^T - (I-K_kH)\hat e_kv_k^TK_k^T \\&- K_kv_k\hat e_k^T(1-K_kH)^T + K_kv_kv_k^TK_k] \} \end{aligned} \]

對括號內每一項求期望得

\[P_k = (1- K_kH)E(\hat e_k\hat e_k^T)(I-K_kH)^T + K_kE(v_kv_k^T)K_k^T \]

令 \(P_k^- = E(\hat e_k\hat e_k^T)\)，稱為系統的預測誤差協方差矩陣，或者估計誤差協方差矩陣；\(R = E(v_kv_k^T)\)，稱為系統的測量誤差協方差矩陣，其之所以沒有 \(k\) 的下標，是因為基本可以認為測量的誤差基本不隨時間變化。

則上式轉化為

\[P_k = (P_k^- - K_kCP_k^-)(I-H^TK_k^T) + K_kRK_k^T \]

要使 \(e_k\) 的方差最小，則使 \(P_k\) 的跡最小，即

\[\frac{\mathrm d\ \mathrm{tr}(P_k)}{\mathrm d K_k} = 0 \]

引入兩個對跡的求導公式

\[\frac{\mathrm d\ \mathrm{tr}(AB)}{\mathrm d A}= B^T\\ \frac{\mathrm{d\ tr}(ABA^T)}{\mathrm{d} A} = 2AB \]

則執行求導：

\[\begin{aligned} \frac{\mathrm{d\ tr}(P_k)}{\mathrm d K_k} &= 0 - 2(HP_k^-)^T + 2K_kHP_k^-C^T + 2K_kR =0\\ K_k &= \frac{P_k^- H^T}{HP_k^-H^T + R} \end{aligned} \]

由此，卡爾曼增益正式被推導出來。