慣性傳感器的卡爾曼濾波

一、引言

下面我們引用文獻【1】中的一段話作爲本文的開始：

想象你在黃昏時分看着一隻小鳥飛行穿過濃密的叢林，你只能隱隱約約、斷斷續續地瞥見小鳥運動的閃現。你試圖努力地猜測小鳥在哪裏以及下一時刻它會出現在哪裏，才不至於失去它的行蹤。或者再想象你是二戰中的一名雷達操作員，正在跟蹤一個微弱的遊移目標，這個目標每隔10秒鐘在屏幕上閃爍一次。或者回到更遠的從前，想象你是開普勒，正試圖根據一組通過不規則和不準確的測量間隔得到的非常不精確的角度觀測值來重新構造行星的運動軌跡。在所有這些情況下，你都試圖根據隨對問變化並且帶有噪聲的觀察數據去估計物理系統的狀態（例如位置、速度等等）。這個問題可以被形式化表示爲時序概率模型上的推理，模型中的轉移模型描述了運動的物理本質，而傳感器模型則描述了測量過程。爲解決這類問題，人們發展出來了一種特殊的表示方法和推理算法——卡爾曼濾波。

二、基本概念

回想一下HMM的基本模型（如下圖所示），其中塗有陰影的圓圈（y_t-2, y_t-1, y_t）相當於是觀測變量，空白圓圈（x_t-2,x_t-1, x_t）相當於是隱變量。這其實揭示了卡爾曼濾波與HMM之間擁有很深的淵源。回到剛剛提及的那幾個例子，你所觀測到的物體狀態（例如雷達中目標的位置或者速度）相當於是對其真實狀態的一種估計（因爲觀測的過程中必然存在噪聲），用數學語言來表述就是P(y_t | x_t)，這就是模型中的測量模型或測量概率（Measurement Probability）。另外一方面，物體當前的（真實）狀態應該與其上一個觀測狀態相關，即存在這樣的一個分佈P(x_t | x_t-1)，這就是模型中的轉移模型或轉移概率（Transition Probability）。當然，HMM中隱變量必須都是離散的，觀測變量並無特殊要求。

而從信號處理的角度來講，濾波是從混合在一起的諸多信號中提取出所需信號的過程[2]。例如，我們有一組含有噪聲的行星運行軌跡，我們希望濾除其中的噪聲，估計行星的真實運動軌跡，這一過程就是濾波。如果從機器學習和數據挖掘的角度來說，濾波是一個理性智能體爲了把握當前狀態以便進行理性決策所採取的行動[1]。比如，前兩天我們沒出門，但是我們可以從房間裏觀察路上的行人有沒有打傘（觀測狀態）來估計前兩天有沒有下雨（真實狀態）。基於這些情況，現在我們要來決策今天（是否會有雨以及）外出是否需要打傘，這個過程就是濾波。讀者應該注意把握上面兩個定義的統一性。

所謂估計就是根據測量得出的與狀態X(t) 有關的數據Y(t) = h[X(t)] + V(t) 解算出X(t)的計算值，其中隨機向量V(t) 爲測量誤差，稱爲X的估計，Y 稱爲 X 的測量。因爲是根據Y(t) 確定的．所以 是Y(t) 的函數。若 是Y 的線性函數，則  稱作 X 的線性估計。設在 [t0, t1] 時間段內的測量爲Y，相應的估計爲，則

• 當t = t1 時，  稱爲X(t)的估計；
• 當t > t1 肘，稱爲X(t)的預測；
• 當t < t1 時， 稱爲X(t)的平滑。

最優估計是指某一指標函數達到最值時的估計。卡爾曼濾波就是一種線性最優濾波器。

因爲後面會用到，這裏我們補充一下關於協方差矩陣的概念。

設 n 維隨機變量（X1, X2, …,Xn）的2階混合中心距

σ_ij = cov(Xi, Xj) = E[(Xi-E(Xi))(Xj-E(Xj))],  (i,j = 1, 2, …, n)

都存在，則稱矩陣

爲 n 維隨機變量（X1, X2, …,Xn）的協方差矩陣，協方差矩陣是一個對稱矩陣，而且對角線是各個維度的方差。

維基百科中還給出了協方差矩陣的一些重要性質，例如下面這兩條（此處不做具體證明）。後續的內容會用到其中的第一條。

三、卡爾曼濾波的方程推導

直接從數學公式和概念入手來考慮卡爾曼濾波無疑是一件非常枯燥的事情。爲了便於理解，我們仍然從一個現實中的實例開始下面的介紹，這一過程中你所需的預備知識僅僅是高中程度的物理學內容。

假如現在有一輛在路上做直線運動的小車（如下所示），該小車在 t 時刻的狀態可以用一個向量來表示，其中pt 表示他當前的位置，vt 表示該車當前的速度。當然，司機還可以踩油門或者剎車來給車一個加速度ut，ut 相當於是一個對車的控制量。顯然，如果司機既沒有踩油門也沒有踩剎車，那麼ut 就等於0。此時車就會做勻速直線運動。

如果我們已知上一時刻 t-1時小車的狀態，現在來考慮當前時刻t 小車的狀態。顯然有

易知，上述兩個公式中，輸出變量都是輸入變量的線性組合，這也就是稱卡爾曼濾波器爲線性濾波器的原因所在。既然上述公式表徵了一種線性關係，那麼我們就可以用一個矩陣來表示它，則有

如果另其中的

則得到卡爾曼濾波方程組中的第一條公式——狀態預測公式，而F就是狀態轉移矩陣，它表示我們如何從上一狀態來推測當前狀態。而B則是控制矩陣，它表示控制量 u 如何作用於當前狀態。

   （1）

上式中x頂上的hat表示爲估計值（而非真實值）。等式左端部分的右上標“-”表示該狀態是根據上一狀態推測而來的，稍後我們還將對其進行修正以得到最優估計，彼時纔可以將“-”去掉。

既然我們是在對真實值進行估計，那麼就理應考慮到噪聲的影響。實踐中，我們通常都是假設噪聲服從一個0均值的高斯分佈，即Noise~Guassian(0, σ)。例如對於一個一維的數據進行估計時，若要引入噪聲的影響，其實只要考慮其中的方差σ即可。當我們將維度提高之後，爲了綜合考慮各個維度偏離其均值的程度，就需要引入協方差矩陣。

回到我們的例子，系統中每一個時刻的不確定性都是通過協方差矩陣 Σ 來給出的。而且這種不確定性在每個時刻間還會進行傳遞。也就是說不僅當前物體的狀態（例如位置或者速度）是會（在每個時刻間）進行傳遞的，而且物體狀態的不確定性也是會（在每個時刻間）進行傳遞的。這種不確定性的傳遞就可以用狀態轉移矩陣來表示，即（注意，這裏用到了前面給出的關於協方差矩陣的性質）

但是我們還應該考慮到，預測模型本身也並不絕對準確的，所以我們要引入一個協方差矩陣 Q 來表示預測模型本身的噪聲（也即是噪聲在傳遞過程中的不確定性），則有

  （2）

這就是卡爾曼濾波方程組中的第二條公式，它表示不確定性在各個時刻間的傳遞關係。

繼續我們的小汽車例子。你應該注意到，前面我們所討論的內容都是圍繞小汽車的真實狀態展開的。而真實狀態我們其實是無法得知的，我們只能通過觀測值來對真實值進行估計。所以現在我們在路上佈設了一個裝置來測定小汽車的位置，觀測到的值記爲Y(t)。而且從小汽車的真實狀態到其觀測狀態還有一個變換關係，這個變換關係我們記爲h(•)，而且這個h(•)還是一個線性函數。此時便有（該式前面曾經給出過）

Y(t) = h[X(t)] + V(t)

其中V(t)表示觀測的誤差。既然h(•)還是一個線性函數，所以我們同樣可以把上式改寫成矩陣的形式，則有

Y_t= Hx_t + v

就本例而言，觀測矩陣 H = [1 0]，這其實告訴我們x和Z的維度不一定非得相同。在我們的例子中，x是一個二維的列向量，而Z只是一個標量。此時當把x與上面給出的H相乘就會得出一個標量，此時得到的 Y 就是x中的首個元素，也就是小車的位置。同樣，我們還需要用一個協方差矩陣R來取代上述式子中的v來表示觀測中的不確定性。當然，由於Z是一個一維的值，所以此時協方差矩陣R也只有一維，也就是隻有一個值，即觀測噪聲之高斯分佈的參數σ。如果我們有很多裝置來測量小汽車的不同狀態，那麼Z就會是一個包含所有觀測值的向量。

接下來要做的事情就是對前面得出的狀態估計進行修正，具體而言就是利用下面這個式子

    （4）

直觀上來說，上式並不難理解。前面我們提到，是根據上一狀態推測而來的，那麼它與“最優”估計值之間的差距現在就是等式右端加號右側的部分。表示實際觀察值與預估的觀測值之間的殘差。這個殘差再乘以一個係數K就可以用來對估計值進行修正。K稱爲卡爾曼係數，它也是一個矩陣，它是對殘差的加權矩陣，有的資料上稱其爲濾波增益陣。

   （3）

上式的推導比較複雜，有興趣深入研究的讀者可以參閱文獻【2】（P35~P37）。如果有時間我會在後面再做詳細推導。但是現在我們仍然可以定性地對這個係數進行解讀：濾波增益陣首先權衡預測狀態協方差矩陣 Σ 和觀測值矩陣R的大小，並以此來覺得我們是更傾向於相信預測模型還是詳細觀測模型。如果相信預測模型多一點，那麼這個殘差的權重就會小一點。反之亦然，如果相信觀察模型多一點，這個殘差的權重就會大一點。不僅如此，濾波增益陣還負責把殘差的表現形式從觀測域轉換到了狀態域。例如本題中觀測值 Z 僅僅是一個一維的向量，狀態 x 是一個二維的向量。所以在實際應用中，觀測值與狀態值所採用的描述特徵或者單位都有可能不同，顯然直接用觀測值的殘差去更新狀態值是不合理的。而利用卡爾曼係數，我們就可以完成這種轉換。例如，在小車運動這個例子中，我們只觀察到了汽車的位置，但K裏面已經包含了協方差矩陣P的信息（P裏面就給出了速度和位置的相關性），所以它利用速度和位置這兩個維度的相關性，從位置的殘差中推算出了速度的殘差。從而讓我們可以對狀態值 x 的兩個維度同時進行修正。

最後，還需對最優估計值的噪聲分佈進行更新。所使用的公式爲

  （5）

至此，我們便獲得了實現卡爾曼濾波所需的全部五個公式，我在前面分別用（1）~（5）的標記進行了編號。我現在把它們再次羅列出來：

我們將這五個公式分成預測組和更新組。預測組總是根據前一個狀態來估計當前狀態。更新組則根據觀測信息來對預測信息進行修正，以期達到最優估計之目的。

四、一個簡單的實例

當然，你可能困惑於卡爾曼濾波是否真的有效。下面利用文獻[4]中給出的例子（爲提升顯示效果，筆者略有修改）來演示卡爾曼濾波的威力。這個例子模擬質點進行勻速直線運動（速度爲1），然後引入一個非常大的噪聲，再用卡爾曼濾波來對質點的運動狀態進行軌跡。注意是勻速直線運動，所以其中不含有控制變量。

Z=(1:100); %觀測值
noise=randn(1,100); %方差爲1的高斯噪聲
Z=Z+noise;

X=[0; 0]; %狀態
Sigma = [1 0; 0 1]; %狀態協方差矩陣
F=[1 1; 0 1]; %狀態轉移矩陣
Q=[0.0001, 0; 0 0.0001]; %狀態轉移協方差矩陣
H=[1 0]; %觀測矩陣
R=1; %觀測噪聲方差

figure;
hold on;

for i=1:100

  X_ = F*X;
  Sigma_ = F*Sigma*F'+Q;
  K = Sigma_*H'/(H*Sigma_*H'+R);
  X = X_+K*(Z(i)-H*X_);
  Sigma = (eye(2)-K*H)*Sigma_;

  plot(X(1), X(2), '.','MarkerSize',10); %畫點，橫軸表示位置，縱軸表示速度
end

plot([0,100],[1,1],'r-');

下圖給出了上述代碼的執行結果。可見經過最開始的幾次迭代後，質點運動的狀態估計就回到了正確軌跡上，而且估計的結果基本圍繞在真實值附近，效果還是很理想的。

五、後記

本文相當於是卡爾曼濾波的入門文，在下一篇中我將深入挖掘一些本文未曾提及的細節（以及一些本文沒有給出的數學上的推導）。如果你想將自己對卡爾曼濾波的認識提升到一個更高的檔次，推薦你關注我的後續博文。Cheers~ 

參考文獻：

【1】Stuart Russell and Peter Norvig. Artificial Intelligence: A Modern Approach. 3rd Edition.

【2】秦永元，張洪鉞，汪叔華，卡爾曼濾波與組合導航原理，西北工業大學出版社

【3】徐亦達博士關於卡爾曼濾波的公開課，http://v.youku.com/v_show/id_XMTM2ODU1MzMzMg.html

【4】卡爾曼濾波的原理以及在MATLAB中的實現，http://blog.csdn.net/revolver/article/details/37830675

我們在上一篇文章中通過一個簡單的例子算是入門卡爾曼濾波了，本文將以此爲基礎討論一些技術細節。

卡爾曼濾波（Kalman Filter）

http://blog.csdn.net/baimafujinji/article/details/50646814

在上一篇文章中，我們已經對HMM和卡爾曼濾波的關聯性進行了初步的討論。參考文獻【3】中將二者之間的關係歸結爲下表。

上表是什麼意思呢？我們其實可以下面的式子來表示，其中，w 和 v 分別表示狀態轉移 和 測量 過程中的不確定性，也即是噪聲，既然是噪聲就可以假設它們服從一個零均值的高斯分佈。這其實跟我們在上一篇文章中所給出的形式是一致的，也就是說我們認爲過去的狀態如果是 x_t-1，那麼當前狀態x_t應該是 x_t-1的一個線性變換，而這個估計過程其實是有誤差的，用一個零均值的高斯噪聲（概率分佈）來表達。類似地，當前的測量值y_t應該是真實值 x_t 的一個線性變換，而這個測量過程仍然是有誤差的，也用一個零均值的高斯噪聲（概率分佈）來表達。

      (1)

上一節中我們還講過，在 [t0, t1] 時間段內的測量爲Y，相應的估計爲，則當t = t1 時，  稱爲X(t)的估計（或者稱爲濾波）。當然現在我們也僅僅需要將注意力放在濾波上，所以最終要求的應該是下面這個式子

根據條件概率的鏈式法則以及馬爾科夫鏈的無記憶性，再去掉常值係數的情況下，就可以得到下面的結論（如果你對有關數學公式記得不是很清楚可以參考http://blog.csdn.net/baimafujinji/article/details/50441927）

其中，P( x_t | y₁, … , y_t-1)就是Prediction（預測），因爲它表示的意義是已知從1到t-1時刻的觀測值y₁, … , y_t-1的情況下求t 時刻的狀態值x_t。另一方面，P( x_t | y₁, … , y_t)就是Update，因爲它表示當我們已經獲得y_t時，再對x_t 進行的一個更新（或修正）。

根據馬爾科夫鏈的無記憶性，可知P( y_t | x_t, y₁, … , y_t-1) = P( y_t | x_t) 。就預測部分而言，我們希望引入x_t-1，所以可以採用下面的方法（這其實就是我們在處理普通貝葉斯網絡時所用過的方法）

到此爲止，其實你應該可以看出來卡爾曼濾波就形成了一個遞歸求解的過程。也就是說，我們欲求P( x_t | y₁, … , y_t-1)，就需要先求P( x_t-1 | y₁, … , y_t-1)，而欲求P( x_t-1 | y₁, … , y_t-1)，就要先求P( x_t-2 | y₁, … , y_t-1) ……結合上一篇文章介紹的內容，其實可以總結卡爾曼濾波的過程如下

也就是說當t = 1時，我們根據觀測值y₁去估計真實狀態x₁，這個過程服從一個高斯分佈。然後，當t = 2時，我們根據上一個觀測值y₁去預測當前的真實狀態x₂，在獲得該時刻的真實觀測值y₂後，我們又可以估計出一個新的真實狀態x₂，這時就要據此對由y₁預測的結果進行修正（Update），如此往復。

接下來，我們引入一個服從零均值高斯分佈的（噪聲）變量 Δx_t-1，

然後試着將Δx_t和Δy_t以Δx_t-1的形式來給出，而且處於方便的考慮，我們忽略掉公式（1）中的控制項 B 和 C，於是有

根據獨立性假設，還可知如下結論（這些都是後續計算推導過程中所需要的準備）：

下面我們要做的事情就是推導卡爾曼濾波的五個公式，在上一篇文章中，我們更多地是從感性的角度給出了這些公式。並沒有給出詳細的數學推導，接下來我們就要來完成這項任務。

綜上我們已經完整地給出了卡爾曼濾波的理論推導。對於結論性的東西，你當然可以直接拿來使用。在一些軟件包中，卡爾曼濾波無非是一條命令或者一個函數就能搞定。我們之所以還在這裏給出它的詳細推導，主要是鑑於這種思想其實在機器學習中也被廣泛地用到，所以瞭解這些技術細節仍然十分有意義。

===================================================================================================

如果你是圖像處理的同道中人，歡迎加入圖像處理算法交流羣（單擊鏈接查看羣號）。

參考文獻：

【1】Stuart Russell and Peter Norvig. Artificial Intelligence: A Modern Approach. 3rd Edition.

【2】秦永元，張洪鉞，汪叔華，卡爾曼濾波與組合導航原理，西北工業大學出版社

【3】徐亦達博士關於卡爾曼濾波的公開課，http://v.youku.com/v_show/id_XMTM2ODU1MzMzMg.html

【4】卡爾曼濾波的原理以及在MATLAB中的實現，http://blog.csdn.net/revolver/article/details/37830675