卡爾曼濾波推導/從一維到多維推導卡爾曼濾波

最近兩天在看組合導航相關內容，談到組合導航，各種濾波技術作爲其重要的數據處理方法，值得我們好好學習一番。首先就從卡爾曼濾波（KF）這個比較成熟且應用廣泛的技術開始吧。本文從一維簡單地例子入手，到多維情況的推導以及卡爾曼濾波如何應用到線性系統中實現狀態的最優估計進行簡單推導並解釋。

爲什麼

關於卡爾曼濾波的通俗理解，可參考其他博客，有很多有趣的解釋，這裏主要給出相對正經的回答。
首先，卡爾曼濾波不同於我們平時所接觸的各種頻域形式的濾波器，比如低通、高通、帶通、帶阻濾波器等等…相比而言，卡爾曼濾波器是一種時域形式的最優估計，給出對一個變量（狀態）的兩個不相關的估計，如何對這兩個估計進行組合以達到最佳估計（最優估計指標爲最小方差）形成一個加權平均是卡爾曼濾波所主要完成的任務。
以導航爲例，常用的導航傳感器有慣導、GPS、視覺等等…但單獨使用某一種傳感器都有所不足，且不構成冗餘配置。以常用的慣導和GPS爲例，慣導系統的優點是：不需要外界提供也不向外輻射任何信息，可工作在任何介質和環境下，且能輸出運載體位置、速度、姿態等信息，系統頻帶寬，輸出數據平穩，短期穩定性好等。但其也有明顯的缺點，即導航精度隨時間而發散，即長期穩定性差。GPS短時間測量精度不如慣導，但GPS不會隨時間漂移，輸出結果相對穩定，但在密閉環境下會丟失信號，且容易受電磁干擾，失去導航能力。所以常用的導航方法是將該兩者結合起來使用，但由於噪聲的存在，兩者的輸出和真值總存在一定的誤差，如何對這兩組數據加權組合以使輸出數據最平穩（方差最小）便是卡爾曼濾波的任務了。

怎麼用

知道了爲什麼要應用卡爾曼濾波，接下來就到了重點：如何進行卡爾曼濾波？爲了便於理解，先從一維的例子開始，後推廣到$n$維。（所涉及到的數學基礎很少，只需瞭解一些統計學基本知識即可）

一維下的卡爾曼濾波

所有涉及多維情況下的算法，最簡單的最容易理解的辦法就是先從一維下手。 $\qquad$ $\qquad$ $\qquad$ $\qquad$ $\qquad$ —我自己

對一個量$x$的兩個不相關估計值$x_1$、$x_2$，其相應的方差爲$\sigma_1^2$和$\sigma_2^2$。要求對這兩個估計值進行組合得到加權平均值，即最優或最小方差估計$\hat x$。通常情況下，這個加權平均值表示如下：
$\hat x_1 = w_1x_1 + w_2x_2 \qquad (1)$ 其中，$w_1$和$w_2$是加權因子且$w_1 + w_2 = 1$。$\hat x$的期望值或均值$E(\hat x)$表示如下：
$E(\hat x) = w_1E(x_1) + w_2E(x_2) \qquad (2)$ $x$的方差定義爲$E[\{ x - E(x)\}^2 ]$，因此$\hat x$的方差$\sigma^2$表示如下：
$\sigma^2 = E\{ ( w_1x_1 + w_2x_2 - w_1E(x_1) - w_2E(x_2) )^2 \} \\ =E\{ w_1^2(x_1-E(x_1))^2 + w_2^2(x_2-E(x_2))^2 \\ - 2w_1w_2(x_1-E(x_1))(x_2-E(x_2)) \} \qquad (3)$由於$x_1$和$x_2$互不相關，則$(x_1-E(x_1))$和$(x_2-E(x_2))$也互不相關，即$E\{(x_1-E(x_1))(x_2-E(x_2))\}=0$。因而$\sigma^2$可表示爲：
$\sigma^2=w_1^2E\{ (x_1-E(x_1))^2 \} + w_2^2E\{ (x_2-E(x_2))^2 \} \\=w_1^2\sigma_1^2+w_2^2\sigma_2^2 \qquad (4)$令$w_2=w$，$w_1=1-w$，方差$\sigma^2$可表示爲：
$\sigma^2=(1-w)^2\sigma_1^2+w^2\sigma_2^2 \qquad (5)$使$\sigma^2$取得最小的$w$值，可通過公式(5)對$w$求微分得到：
$\frac{d\sigma^2}{dw} = -2(1-w)\sigma_1^2+2w\sigma_2^2=0$由上式可求得最優的加權因子：
$w= \frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2} \qquad (6)$將式（6）代入式（1）、式（5），可得到$\hat x$和它的方差$\sigma^2$:
$\hat x = \frac{\sigma_2^2x_1+\sigma_1^2x_2}{\sigma_1^2+\sigma_2^2} \qquad(7)$$\sigma^2= \frac{\sigma_1^2\sigma_2^2}{\sigma_1^2+\sigma_2^2} \qquad(8)$通過上面的運算，兩個不相關的估計值$x_1$和$x_2$經過組合得到了加權平均值。加權平均值根據最小方差來選取。在卡爾曼濾波器中，估計值通常通過兩種途徑獲得，其一是根據已知的運動方程，對前一次的最優估計進行更新；另一種途徑是從測量值來獲取估計值。若把$x_2$看做測量值，並且用來改善被更新的估計值$x_1$，則上面的方程可以表示爲如下形式：
$\hat x = x_1-w(x_1-x_2) \qquad (9)$$\sigma^2=\sigma_1^2(1-w) \qquad (10)$這說明了如何利用測量值（$x_2$）來改善估計值（$x_1$）和它的方差（$\sigma_1^2$）。這個過程可以推廣到完整卡爾曼濾波的多維形式。

推廣到多維卡爾曼濾波

現在，考慮一個$n$維矢量$x$，它的兩個不相關估計值爲$x_1$和$x_2$，其方差分別用兩個$n \times n$的矩陣$P_1$、$P_2$來表示。
$x_1$和$x_2$的加權均值可以用如前面討論的一維情況同樣的形式來表示：
$\hat x = (I-W)x_1+Wx_2 = x_1-W(x_1-x_2) \qquad (11)$式中：$W$是一個$n \times n$階加權陣；$I$是同階單位陣；$\hat x$表示$x$的最優估計。當$W$的取值使$\hat x$的方差最小時，可以通過式（11）得到$x$的最優估計值$\hat x$。
在許多實際應用中，兩個估計值的維數並不相等。例如，用$y_2$表示$m$個測量值，而$y_2$只和$x$中的某些元素有關，在這種情況下，$y_2$和$x_2$的關係可以用下式表示：
$y_2=Hx_2 \qquad (12)$式中：$H$是一個$m\times n$階矩陣。
因此，可以從估計值$x_1$（方差爲$P_1$）和估計值$y_2$（=$H_2x_2$，方差用R表示）得到$x$的最優估計。令加權矩陣$W=KH$，式中$K$是另一任意的加權矩陣，則：
$\hat x=x_1-KH(x_1-x_2)=x_1-K(Hx_1-y_2)\\=(I-KH)x_1+Ky_2 \qquad(13)$由方差的定義可得$\hat x$的方差$P$:
$P=E\{ [\hat x -E(\hat x)][\hat x -E(\hat x)]^T \} \qquad (14)$類似地可得到方差$P_1$和$R$。把方程（13）代入式（14），可得$P=E\{ [(I-KH)x_1+Ky_2-(I-KH)E(x_1)\\-KE(Y_2)] - [(I-KH)x_1+Ky_2-(I-KH)E(x_1)\\-KE(Y_2)] ^T \}$由於$x_1$和$y_2$不相關，上式可簡化爲：
$P=(I-KH)E\{ [x_1-E(x_1)][x_1-E(x_1)]^T(I-KH)^T \}\\+KE \{ [y_2-E(y_2)][y_2-E(y_2)]^T \}K^T\\=(I-KH)P_1(I-KH)^T+KRK^T\qquad(15)$現在要尋找能使方差陣$P$最小的$K$值，即使$x$的方差陣$P$對角線上的元素最小。
參考文獻【1】中提供了滿足條件的$K$值：
$K=P_1H^T[HP_1H^T+R]^{-1} \qquad(16)$在這個條件下，$x$的最優估計表示爲：
$\hat x = x_1-K[Hx_1-y_2]\qquad(17)$方差表示爲：
$P=P_1-KHP_1\qquad(18)$式中，$K$由方程（16）給出。式（16）~（18）定義的加權過程如何應用於卡爾瑪女博器，將在下面討論。

將卡爾曼濾波應用於線性系統

前面所講的多維情況是假設只有一組估計值來進行最優估計的求解，而對於一個實際運行的系統來說，系統的狀態是持續不斷到的更新的，這就需要來做一些處理使上面的算法可以持續運行下去，下面我們就來講解這部分內ring。卡爾曼濾波適用於高斯噪聲下的線性系統（若不滿足，可考慮其改進算法及其他濾波技術，後面可能會更新這部分）。

線性系統

一個線性系統的動態特性可以用一組一屆的微分方程來描述如下：
$\dot x=Fx+Gu+Dw\qquad (19)$式中：$x(t)$爲$n$維繫統狀態矢量；$u(t)$爲$p$維確定性輸入矢量；$w(t)$爲系統噪聲；$F$是$n\times n$階系統矩陣；$G$是$n\times p$階系統輸入矩陣；$F$、$G$和$D$是常值或時變矩陣。系統噪聲$w(t)$均值爲零且成高斯分佈，功率譜密度爲$Q$。
假設系統有$m$個測量值，是狀態變量$x(t)$的線性組合，但包含測量噪聲。測量值可以用系統狀態變量表示如下：
$y=Hx+n \qquad (20)$式中：$y(t)$爲$m$維的測量矢量；$H$是一個$m\times n$階的測量矩陣；$n(t)$表示零均值高斯分佈的測量噪聲，功率譜密度爲$R$。
上述系統用卡爾曼濾波器來求得系統狀態變量$x$的最優估計，已知：

1. 測量值$y$；
2. 由矩陣$F$、$G$、$H$、$D$確定的系統模型；
3. 已知系統噪聲和測量噪聲的統計特性矩陣$Q$和$R$。

確定性或可測量的輸入通過系統和系統模型來進行處理，如下圖所示。
真實系統的測量值和系統的預估值進行比較，系統預估值來自系統模型提供的狀態變量的最新最優估計。真值和預估值之間的差異通過一個加權陣，即卡爾曼增益陣，反饋給系統，來校正系統模型的狀態估計值。
通常，我們所需的算法爲離散形式，故將上述連續方程表示爲差分方程形式，如下所示：
$x_{k+1}=\Phi_kx_k+\Gamma_ku_k+\Delta_kw_k\qquad(21)$$y_{k+1}=H_{k+1}x_{k+1}+n_{k+1}\qquad(22)$式中：$x_k$爲$t_k$時刻的狀態矢量；$u_k$爲$t_k$時刻的輸入；$w_k$爲$t_k$時刻的系統噪聲；$n_{k+1}$爲$t_{k+1}$時刻的測量噪聲；$\Phi_k$爲$t_k$至$t_{k+1}$時刻的狀態轉移矩陣；$H_{k+1}$爲$t_{k+1}$時刻的測量矩陣；$\Gamma_k$和$\Delta_k$爲適當的輸入矩陣。
噪聲是離散的零均值噪聲，其協方差陣分別爲$Q_k$和$R_k$。
這些方程用於構成一個遞推的卡爾曼濾波算法。在這些公式中，需要考慮兩組不同的方程。第一組是基於上一步系統狀態最優估計的預測方程，另一組是通過把預測值與新的測量值進行組合，來對預測的最優估計進行更新。

預測更新

在$t_k$時刻的狀態變量$x_k$的最優估計用$x_{k/k}$來表示。由於系統具有零均值的白噪聲$w_k$，$t_{k+1}$時刻狀態變量的最優預測爲：
$x_{k+1/k}=\Phi_kx_{k/k}\qquad(23)$$t_{k+1}$時刻協方差陣的期望值通過$t_k$時刻的協方差預測：
$P_{k+1/k}=\Phi_kp_{k/k}\Phi_k^T+\Delta_kQ_k\Delta_k^T\qquad(24)$

測量更新

$t_{k+1}$時刻新的測量值$y_{k+1}$，與來自系統模型的測量值的預測值進行比較，根據上面的算法，用測量值對預測值進行更新，以獲得一個最優估計。因此$t_{k+1}$時刻的狀態變量的最優估計如下：
$x_{k+1/k+1}=x_{k+1/k}-K_{k+1}[H_{k+1}x_{k+1/k}-y_{k+1}]\qquad(25)$其協方差爲：
$P_{k+1/k+1}=P_{k+1/k}-K_{k+1}H_{k+1}P_{k+1/k}\qquad(26)$式中卡爾曼增益陣爲：
$K_{k+1}=P_{k+1/k}H_{k+1}^T[H_{k+1}P_{k+1/k}H_{k+1}^T+R_{k+1}]^{-1}\qquad(27)$這樣，每次系統採集到新的測量值，就可利用式（25）~（27）對系統狀態進行更新。

注：本文參考國防工業出版社的《捷聯慣性導航技術 第二版》，張天光等譯。