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

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

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

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

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

        如果另其中的

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

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

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

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

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

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

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

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

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

                                                                                      Yt=Hxt + v

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

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

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

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

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

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

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