Kalman filter公式推導

Kalman濾波是一種基于測量結果更新預測值，以獲得對系統狀态的更精準估計的算法。下圖給出了Kalman濾波算法的基本步驟（摘自Wikipedia），本文将對Kalman filter算法中所涉及到的幾條公式進行推導。

如假定所研究的系統為時序離散動力學體系，那麼 k k k時刻系統狀态 x k x_{k} xk​應由下式決定：

x k = F k x k − 1 + B k u k + w k                             ( 1 ) x_{k} = F_{k} x_{k - 1} + B_{k} u_{k} + w_{k}~~~~~~~~~~~~~~~~~~~~~~~~~~~ (1) xk​=Fk​xk−1​+Bk​uk​+wk​                           (1)

其中， F k F_{k} Fk​為系統狀态轉移矩陣， B k B_{k} Bk​則為控制輸入矩陣， u k u_{k} uk​是系統的輸入向量， w k w_{k} wk​則表示過程噪聲( w k ∼ N ( 0 , Q k ) w_{k} \sim \mathscr{N} (0, Q_{k}) wk​∼N(0,Qk​))。

實際上，我們所得到的系統狀态應被表示為 z k z_{k} zk​，它其實是由觀測者對體系的測量所獲得的結果，即

z k = H k x k + v k                             ( 2 ) z_{k} = H_{k} x_{k} + v_{k}~~~~~~~~~~~~~~~~~~~~~~~~~~~ (2) zk​=Hk​xk​+vk​                           (2)

其中， H k H_{k} Hk​表示狀态空間向觀測空間映射的狀态觀測矩陣， v k v_{k} vk​則為測量噪聲( v k ∼ N ( 0 , R k ) v_{k} \sim \mathscr{N} (0, R_{k}) vk​∼N(0,Rk​))。

一般來說，如系統的狀态演化符合Markov過程，那麼基于 k k k時刻之前的狀态，我們可以對該時刻的系統狀态進行估計，由式(1)可得：

x ^ k ∣ k − 1 = F k x ^ k − 1 ∣ k − 1 + B k u k                        ( 3 ) \hat{x}_{k | k - 1} = F_{k} \hat{x}_{k-1 | k-1} + B_{k} u_{k}~~~~~~~~~~~~~~~~~~~~~~(3) x^k∣k−1​=Fk​x^k−1∣k−1​+Bk​uk​                      (3)

其中， x ^ k ∣ k − 1 \hat{x}_{k | k - 1} x^k∣k−1​為 k k k時刻的先驗狀态估計值， x ^ k − 1 ∣ k − 1 \hat{x}_{k - 1 | k - 1} x^k−1∣k−1​則為 k − 1 k - 1 k−1時刻的後驗狀态估計值。

由此，在系統狀态估計過程中的模型預測誤差可被定義為：

e r r k p r e d = x ^ k ∣ k − x ^ k ∣ k − 1 err^{pred}_{k} = \hat{x}_{k | k} - \hat{x}_{k | k-1} errkpred​=x^k∣k​−x^k∣k−1​

而根據式(2)，該過程中總的誤差可被定義為：

e r r k a l l = e r r k p r e d + e r r k m e a s u r e = z k − H k x ^ k ∣ k − 1 err^{all}_{k} = err^{pred}_{k} + err^{measure}_{k} = z_{k} - H_{k} \hat{x}_{k | k - 1} errkall​=errkpred​+errkmeasure​=zk​−Hk​x^k∣k−1​

這樣，我們将Kalman增益系數 K k K_{k} Kk​表示為 k k k時刻，模型預測誤差在總誤差中的比重，即

K k = e r r k p r e d e r r k a l l = x ^ k ∣ k − x ^ k ∣ k − 1 z k − H k x ^ k ∣ k − 1 K_{k} = \dfrac{err^{pred}_{k}}{err^{all}_{k}} = \dfrac{\hat{x}_{k | k} - \hat{x}_{k | k-1}}{z_{k} - H_{k} \hat{x}_{k | k - 1}} Kk​=errkall​errkpred​​=zk​−Hk​x^k∣k−1​x^k∣k​−x^k∣k−1​​

将式(2)代入上式可得：

x ^ k ∣ k = x ^ k ∣ k − 1 + K k [ H k ( x k − x ^ k ∣ k − 1 ) + v k ] \hat{x}_{k | k} = \hat{x}_{k | k - 1} + K_{k} [H_{k} (x_{k} - \hat{x}_{k | k - 1}) + v_{k}] x^k∣k​=x^k∣k−1​+Kk​[Hk​(xk​−x^k∣k−1​)+vk​]

由此可得：

x k − x ^ k ∣ k = ( I − K k H k ) ( x k − x ^ k ∣ k − 1 ) − K k v k x_{k} - \hat{x}_{k | k} = (I - K_{k} H_{k}) (x_{k} - \hat{x}_{k | k - 1}) - K_{k} v_{k} xk​−x^k∣k​=(I−Kk​Hk​)(xk​−x^k∣k−1​)−Kk​vk​

是以，系統的真實狀态 x k x_{k} xk​與後驗狀态估計值 x ^ k ∣ k \hat{x}_{k | k} x^k∣k​之間的協方差可以表示為：

P k ∣ k = c o v ( x k − x ^ k ∣ k ) = c o v [ ( I − K k H k ) ( x k − x ^ k ∣ k − 1 ) − K k v k ] P_{k | k} = \rm{cov} (x_{k} - \hat{x}_{k | k}) = \rm{cov} [(I - K_{k} H_{k}) (x_{k} - \hat{x}_{k | k - 1}) - K_{k} v_{k}] Pk∣k​=cov(xk​−x^k∣k​)=cov[(I−Kk​Hk​)(xk​−x^k∣k−1​)−Kk​vk​]

考慮到協方差矩陣的計算公式，上式可被改寫為：

P k ∣ k = ( I − K k H k ) c o v ( x k − x ^ k ∣ k − 1 ) ( I − K k H k ) T + K k c o v ( v k ) K k T = ( I − K k H k ) P k ∣ k − 1 ( I − K k H k ) T + K k R k K k T P_{k | k} = (I - K_{k} H_{k}) \rm{cov} (x_{k} - \hat{x}_{k | k - 1}) (I - K_{k} H_{k})^{T} + K_{k} \rm{cov} (v_{k}) K^{T}_{k} = (I - K_{k} H_{k}) P_{k | k - 1} (I - K_{k} H_{k})^{T} + K_{k} R_{k} K^{T}_{k} Pk∣k​=(I−Kk​Hk​)cov(xk​−x^k∣k−1​)(I−Kk​Hk​)T+Kk​cov(vk​)KkT​=(I−Kk​Hk​)Pk∣k−1​(I−Kk​Hk​)T+Kk​Rk​KkT​

展開上式，我們可得：

P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 H k T K k T + K k H k P k ∣ k − 1 H k T K k T + K k R k K k T                                   ( 4 ) P_{k | k} = P_{k | k - 1} - K_{k} H_{k} P_{k | k - 1} - P_{k | k - 1} H^{T}_{k} K^{T}_{k} + K_{k} H_{k} P_{k | k - 1} H^{T}_{k} K^{T}_{k} + K_{k} R_{k} K^{T}_{k}~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~(4) Pk∣k​=Pk∣k−1​−Kk​Hk​Pk∣k−1​−Pk∣k−1​HkT​KkT​+Kk​Hk​Pk∣k−1​HkT​KkT​+Kk​Rk​KkT​                                 (4)

顯然，我們期望 x k = x ^ k ∣ k x_{k} = \hat{x}_{k | k} xk​=x^k∣k​，即協方差 P k ∣ k P_{k | k} Pk∣k​關于 K k K_{k} Kk​的導數為零：

∂ P k ∣ k ∂ K k = − 2 H k P k ∣ k − 1 + 2 ( H k P k ∣ k − 1 H k T + R k ) K k T = 0 ⟹ K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1     ( 5 ) \dfrac{\partial P_{k | k}}{\partial K_{k}} = - 2 H_{k} P_{k | k - 1} + 2 (H_{k} P_{k | k - 1} H^{T}_{k} + R_{k}) K^{T}_{k} = 0 \Longrightarrow K_{k} = P_{k | k - 1} H^{T}_{k} (H_{k} P_{k | k - 1} H^{T}_{k} + R_{k})^{-1}~~~(5) ∂Kk​∂Pk∣k​​=−2Hk​Pk∣k−1​+2(Hk​Pk∣k−1​HkT​+Rk​)KkT​=0⟹Kk​=Pk∣k−1​HkT​(Hk​Pk∣k−1​HkT​+Rk​)−1   (5)

将式(5)代入式(4)，可以得到：

P k ∣ k = ( I − K k H k ) P k ∣ k − 1                                 ( 6 ) P_{k | k} = (I - K_{k} H_{k}) P_{k | k - 1}~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~(6) Pk∣k​=(I−Kk​Hk​)Pk∣k−1​                               (6)

此外，将式(1)與式(3)的等号兩邊相減可得：

x k − x ^ k ∣ k − 1 = F k ( x k − 1 − x ^ k − 1 ∣ k − 1 ) + w k x_{k} - \hat{x}_{k | k - 1} = F_{k} (x_{k - 1} - \hat{x}_{k - 1 | k - 1}) + w_{k} xk​−x^k∣k−1​=Fk​(xk−1​−x^k−1∣k−1​)+wk​

于是，

P k ∣ k − 1 = c o v ( x k − x ^ k ∣ k − 1 ) = c o v [ F k ( x k − 1 − x ^ k − 1 ∣ k − 1 ) ] + c o v ( w k ) = F k P k − 1 ∣ k − 1 F k T + Q k       ( 7 ) P_{k | k - 1} = \rm{cov} (x_{k} - \hat{x}_{k | k - 1}) = \rm{cov} [F_{k} (x_{k - 1} - \hat{x}_{k - 1 | k - 1})] + \rm{cov} (w_{k}) = F_{k} P_{k - 1 | k - 1} F^{T}_{k} + Q_{k}~~~~~(7) Pk∣k−1​=cov(xk​−x^k∣k−1​)=cov[Fk​(xk−1​−x^k−1∣k−1​)]+cov(wk​)=Fk​Pk−1∣k−1​FkT​+Qk​     (7)

至此，我們便得到了Kalman濾波算法中所涉及的各公式的推導過程。根據本文一開始引用的圖，我們可以給出Kalman濾波算法的基本步驟：

（1）已知系統的狀态轉移矩陣 F k F_{k} Fk​、控制輸入矩陣 B k B_{k} Bk​、輸入向量 u k u_{k} uk​以及過程噪聲的協方差矩陣 Q k Q_{k} Qk​，在對應于 k − 1 k - 1 k−1時刻的 P k − 1 ∣ k − 1 P_{k - 1 | k - 1} Pk−1∣k−1​和 x ^ k − 1 ∣ k − 1 \hat{x}_{k - 1 | k - 1} x^k−1∣k−1​的基礎上，可獲得對 k k k時刻的先驗狀态估計值：

x ^ k ∣ k − 1 = F k x ^ k − 1 ∣ k − 1 + B k u k \hat{x}_{k | k -1} = F_{k} \hat{x}_{k - 1 | k - 1} + B_{k} u_{k} x^k∣k−1​=Fk​x^k−1∣k−1​+Bk​uk​

利用式(7)，我們可以得到：

P k ∣ k − 1 = F k P k − 1 ∣ k − 1 F k T + Q k P_{k | k - 1} = F_{k} P_{k - 1 | k - 1} F^{T}_{k} + Q_{k} Pk∣k−1​=Fk​Pk−1∣k−1​FkT​+Qk​

（2）基于式(5)和式(6)，我們可以得到 k k k時刻的Kalman增益系數：

K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1 K_{k} = P_{k | k - 1} H^{T}_{k} (H_{k} P_{k | k - 1} H^{T}_{k} + R_{k})^{-1} Kk​=Pk∣k−1​HkT​(Hk​Pk∣k−1​HkT​+Rk​)−1

以及協方差

P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k | k} = (I - K_{k} H_{k}) P_{k | k - 1} Pk∣k​=(I−Kk​Hk​)Pk∣k−1​

（3）在獲得測量值 z k z_{k} zk​後，後驗狀态估計值 x ^ k ∣ k \hat{x}_{k | k} x^k∣k​可由Kalman增益系數的定義式算得：

x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) \hat{x}_{k | k} = \hat{x}_{k | k - 1} + K_{k} (z_{k} - H_{k} \hat{x}_{k | k - 1}) x^k∣k​=x^k∣k−1​+Kk​(zk​−Hk​x^k∣k−1​)

（4）由此，我們得到 k k k時刻對應的 x ^ k ∣ k \hat{x}_{k | k} x^k∣k​和 P k ∣ k P_{k | k} Pk∣k​。重複上述(1)~(3)步，便可獲得 k + 1 k + 1 k+1時刻的相應值。