控制算法學習 一、卡爾曼濾波（3）卡爾曼增益推導

• 前言
• 系統模組化
• 狀态預測
• 最優估計問題
• 卡爾曼增益
• 後記

前言

前兩次卡爾曼濾波，分别寫了卡爾曼濾波的兩大流程（預測、更新）兩個方程五個公式，和卡爾曼濾波與貝葉斯濾波的關系。

本次将着重于從機率角度上推導卡爾曼濾波的五個公式，以及卡爾曼增益。

系統模組化

設線性系統狀态 X = ( x 1 , x 2 , . . . … , x n ) X=(x_1,x_2,...\dots,x_n) X=(x1​,x2​,...…,xn​)，上一時刻的狀态最優估計是 x ^ n − 1 ∼ N ( μ ^ n − 1 , P ^ n − 1 ) \hat x_{n-1} \sim N(\hat\mu_{n-1},\hat P_{n-1}) x^n−1​∼N(μ^​n−1​,P^n−1​)，本時刻的觀測狀态是 z ^ n \hat z_n z^n​。假設系統滿足馬爾可夫性，求目前時刻的狀态最優估計 x ^ n \hat x_n x^n​

狀态方程與觀測方程：（後續不考慮控制輸入矩陣）

x n = A x n − 1 + B u n + w n , w n ∼ N ( 0 , R ) z n = H x n + v n , v n ∼ N ( 0 , Q ) x_n = Ax_{n-1}+Bu_n+w_n,w_n \sim N(0, R) \\ z_n = Hx_n+v_n,v_n \sim N(0,Q) xn​=Axn−1​+Bun​+wn​,wn​∼N(0,R)zn​=Hxn​+vn​,vn​∼N(0,Q)

狀态預測

目前時刻的狀态預測可以通過狀态方程與上一時刻的最優估計獲得：

x ^ n ∣ n − 1 = A x ^ n − 1 + w n \hat x_{n|n-1} = A\hat x_{n-1} + w_n \\ x^n∣n−1​=Ax^n−1​+wn​

由于 x ^ n − 1 ∼ N ( μ ^ n − 1 , P ^ n − 1 ) \hat x_{n-1} \sim N(\hat\mu_{n-1},\hat P_{n-1}) x^n−1​∼N(μ^​n−1​,P^n−1​)，由機率分布期望和協方差矩陣的性質，可知：

x ^ n ∣ n − 1 ∼ N ( A μ ^ n − 1 , A P ^ n − 1 A T + R ) \hat x_{n|n-1} \sim N(A\hat \mu_{n-1},A\hat P_{n-1}A^T+R) x^n∣n−1​∼N(Aμ^​n−1​,AP^n−1​AT+R)

當期時刻的觀測預測可以通過觀測方程和狀态預測獲得：

z ^ n ∣ n − 1 = H x ^ n ∣ n − 1 + v n \hat z_{n|n-1}=H \hat x_{n|n-1}+v_n z^n∣n−1​=Hx^n∣n−1​+vn​

由機率分布期望和協方差矩陣的性質可得：

z ^ n ∣ n − 1 ∼ N ( H A μ ^ n − 1 , H A P ^ n − 1 A T H T + H R ) \hat z_{n|n-1}\sim N(HA\hat \mu_{n-1}, HA\hat P_{n-1}A^TH^T + HR) z^n∣n−1​∼N(HAμ^​n−1​,HAP^n−1​ATHT+HR)

需要注意的是，觀測預測沒有觀測噪聲，因為觀測噪聲是傳感器帶來的，而預測并不涉及傳感器。

最優估計問題

本時刻的觀測為 z ^ n \hat z_n z^n​，觀測預測為 z ^ n ∣ n − 1 \hat z_{n|n-1} z^n∣n−1​，二者之間的內插補點就是觀測殘差：

Δ z ^ n = z ^ n − z ^ n ∣ n − 1 \Delta \hat z_n = \hat z_n - \hat z_{n|n-1} \\ Δz^n​=z^n​−z^n∣n−1​

觀測殘差的意義是，本時刻的觀測與預測的偏差，而卡爾曼增益，就是通過預測量與觀測量偏差進行權重，獲得最優估計：

x ^ n = x ^ n ∣ n − 1 + K k Δ z ^ n x ^ n ∼ N ( E ( x ^ n ∣ n − 1 ) + K k E ( Δ z ^ n ) , P ^ n ∣ n − 1 + K k P ^ Δ z ^ n K k T ) \hat x_n = \hat x_{n|n-1}+K_k \Delta \hat z_n \\ \hat x_n \sim N(E(\hat x_{n|n-1})+K_kE(\Delta \hat z_n),\hat P_{n|n-1}+K_k \hat P_{\Delta\hat z_n}K_k^T) x^n​=x^n∣n−1​+Kk​Δz^n​x^n​∼N(E(x^n∣n−1​)+Kk​E(Δz^n​),P^n∣n−1​+Kk​P^Δz^n​​KkT​)

此外，卡爾曼增益不僅代表了觀測偏差的權，也負責将觀測量線性變換到狀态量的尺度（這是因為在不同系統中，觀測與狀态的範圍不一定相同）。

現在的問題是，什麼是最優估計？或者說，怎樣的 x ^ n \hat x_{n} x^n​才是 x n x_n xn​的最優估計？

答案是：與 x n x_n xn​相差最小的 x ^ n \hat x_{n} x^n​就是最優估計，可以用以下公式來衡量：

arg min ⁡ x ^ n E ( ∥ x ^ n − x n ∥ 2 ) \argmin_{\hat x_n} E(\Vert \hat x_n-x_n \Vert^2) x^n​argmin​E(∥x^n​−xn​∥2)

其中， x n ∈ R n x_n \in R^n xn​∈Rn（也就是說真實狀态是一個常向量，定值）。

向量範數的最小值問題很難求解，涉及到優化理論。那麼将以上式子進行變換：

E ( ∥ x ^ n − x n ∥ 2 ) = t r [ E ( x ^ n − x n ) ( x ^ n − x n ) T ] = t r [ C o v ( x ^ n − x n , x ^ n − x n ) ] E(\Vert \hat x_n-x_n \Vert^2)=tr[E(\hat x_n-x_n)(\hat x_n-x_n)^T] \\ = tr[Cov(\hat x_n-x_n, \hat x_n-x_n)] E(∥x^n​−xn​∥2)=tr[E(x^n​−xn​)(x^n​−xn​)T]=tr[Cov(x^n​−xn​,x^n​−xn​)]

右邊的式子是 x ^ n − x n \hat x_n-x_n x^n​−xn​的協方差矩陣，協方差矩陣的對角線元素就是 x ^ n − x n \hat x_n-x_n x^n​−xn​各分量的方差，這裡以分量 x 1 為 例 x_1為例 x1​為例：

V a r ( x ^ 1 − x 1 ) = E ( x ^ 1 − x 1 − E ( x ^ 1 − x 1 ) ) 2 = E ( x ^ 1 − x 1 ) 2 Var(\hat x_1-x_1)=E(\hat x_1-x_1 - E(\hat x_1-x_1))^2\\ = E(\hat x_1-x_1)^2 Var(x^1​−x1​)=E(x^1​−x1​−E(x^1​−x1​))2=E(x^1​−x1​)2

需要解釋為什麼 E ( x ^ 1 − x 1 ) = 0 E(\hat x_1-x_1)=0 E(x^1​−x1​)=0，這是因為卡爾曼濾波是無偏估計，即狀态最優估計與真實狀态的期望相同。

以上推導将最優估計與 t r [ E ( x ^ n − x n ) ( x ^ n − x n ) T ] tr[E(\hat x_n-x_n)(\hat x_n-x_n)^T] tr[E(x^n​−xn​)(x^n​−xn​)T]挂鈎。現在我們的任務就是最小化這個協方差矩陣的迹：

arg min ⁡ x ^ n t r [ C o v ( x ^ n − x n , x ^ n − x n ) ] \argmin_{\hat x_n} tr[Cov(\hat x_n-x_n, \hat x_n-x_n)] x^n​argmin​tr[Cov(x^n​−xn​,x^n​−xn​)]

卡爾曼增益

根據多元随機變量協方差的線性性質，簡化 x ^ n − x n \hat x_n-x_n x^n​−xn​的協方差矩陣：

C o v ( x ^ n − x n , x ^ n − x n ) = C o v ( x ^ n , x ^ n ) t r [ C o v ( x ^ n − x n , x ^ n − x n ) ] = t r [ C o v ( x ^ n , x ^ n ) ] Cov(\hat x_n-x_n,\hat x_n-x_n)=Cov(\hat x_n,\hat x_n) \\ tr[Cov(\hat x_n-x_n,\hat x_n-x_n)]=tr[Cov(\hat x_n,\hat x_n)] Cov(x^n​−xn​,x^n​−xn​)=Cov(x^n​,x^n​)tr[Cov(x^n​−xn​,x^n​−xn​)]=tr[Cov(x^n​,x^n​)]

至此，卡爾曼濾波中的狀态最優估計問題，被轉換為最小化最優估計的協方差矩陣的迹，而協方差矩陣的迹則通過卡爾曼增益來調整。 這就是各大部落格資料裡所謂的協方差矩陣均方誤差最小化的本質。

接下來就要來解這個最小化問題。首先重新整理一下最優估計的公式：

x ^ n = x ^ n ∣ n − 1 + K k Δ z ^ n = x ^ n ∣ n − 1 + K k z ^ n − K k H x ^ n ∣ n − 1 = ( I − K k H ) x ^ n ∣ n − 1 + K k H x n + K k v n \hat x_n = \hat x_{n|n-1}+K_k \Delta \hat z_n \\ = \hat x_{n|n-1}+K_k\hat z_n-K_kH \hat x_{n|n-1} \\ = (I-K_kH)\hat x_{n|n-1}+K_kHx_n + K_kv_n x^n​=x^n∣n−1​+Kk​Δz^n​=x^n∣n−1​+Kk​z^n​−Kk​Hx^n∣n−1​=(I−Kk​H)x^n∣n−1​+Kk​Hxn​+Kk​vn​

t r [ C o v ( x ^ n , x ^ n ) ] = t r [ ( I − K k H ) P ^ n ∣ n − 1 ( I − K k H ) T + K k Q K k T ] = t r [ P n ∣ n − 1 − K k H P n ∣ n − 1 − P n ∣ n − 1 H T K k T + K k H P n ∣ n − 1 H T K k T + K k Q K k T ] tr[Cov(\hat x_n,\hat x_n)] = tr[(I-K_kH)\hat P_{n|n-1}(I-K_kH)^T + K_kQK_k^T] \\ = tr[P_{n|n-1}-K_kHP_{n|n-1}-P_{n|n-1}H^TK_k^T+K_kHP_{n|n-1}H^TK_k^T + K_kQK_k^T] \\ tr[Cov(x^n​,x^n​)]=tr[(I−Kk​H)P^n∣n−1​(I−Kk​H)T+Kk​QKkT​]=tr[Pn∣n−1​−Kk​HPn∣n−1​−Pn∣n−1​HTKkT​+Kk​HPn∣n−1​HTKkT​+Kk​QKkT​]

現在就可以計算卡爾曼增益了：

∂ t r [ C o v ( x ^ n , x ^ n ) ] ∂ K k = − ( H P n ∣ n − 1 ) T − ( H P n ∣ n − 1 ) T + 2 K k ( H P n ∣ n − 1 H T + Q ) 2 ( H P n ∣ n − 1 ) T = 2 K k ( H P n ∣ n − 1 H T + Q ) K k = P n ∣ n − 1 T H T ( H P n ∣ n − 1 H T + Q ) − 1 \frac {\partial tr[Cov(\hat x_n,\hat x_n)]}{\partial K_k} = -(HP_{n|n-1})^T-(HP_{n|n-1})^T+ 2K_k(HP_{n|n-1}H^T+Q) \\ 2(HP_{n|n-1})^T=2K_k(HP_{n|n-1}H^T+Q) \\ K_k = P_{n|n-1}^TH^T(HP_{n|n-1}H^T+Q)^{-1} ∂Kk​∂tr[Cov(x^n​,x^n​)]​=−(HPn∣n−1​)T−(HPn∣n−1​)T+2Kk​(HPn∣n−1​HT+Q)2(HPn∣n−1​)T=2Kk​(HPn∣n−1​HT+Q)Kk​=Pn∣n−1T​HT(HPn∣n−1​HT+Q)−1

獲得卡爾曼增益 K k K_k Kk​後，代入式 x ^ n = x ^ n ∣ n − 1 + K k Δ z ^ n \hat x_n = \hat x_{n|n-1}+K_k \Delta \hat z_n x^n​=x^n∣n−1​+Kk​Δz^n​，就得到了狀态最優估計，目前時刻的卡爾曼濾波就完成了。

後記

以上就是卡爾曼濾波中，卡爾曼增益的推導了，着重在于最優估計與協方差矩陣的關系解讀。

最後求協方差矩陣的迹的偏導時，需要矩陣求導相關的基礎，過幾天會在機率論專欄更新矩陣求導對應的内容。

推到中還提到了無偏估計，下一次将更新卡爾曼濾波的無偏估計性質以及證明。

最後，線性卡爾曼濾波即将結束，下下次會進入擴充卡爾曼濾波EKF的記錄，并且後續會延伸到IKF、UKF、ESKF等相關算法。