卡爾曼濾波的工作原理

　　在學習卡爾曼濾波器之前，首先看看為什么叫“卡爾曼”。跟其他著名的理論（例如傅立葉變換，泰勒級數等等）一樣，卡爾曼也是一個人的名字，而跟他們不同的是，他是個現代人！

　　卡爾曼全名Rudolf Emil Kalman，匈牙利數學家，1930年出生于匈牙利首都布達佩斯。1953，1954年于麻省理工學院分別獲得電機工程學士及碩士學位。1957年于哥倫比亞大學獲得博士學位。我們現在要學習的卡爾曼濾波器，正是源于他的博士論文和1960年發表的論文《A New Approach to Linear Filtering and Prediction Problems》（線性濾波與預測問題的新方法）。

　　簡單來說，卡爾曼濾波器是一個“optimal recursive data processing algorithm（最優化自回歸數據處理算法）”。對于解決很大部分的問題，他是最優，效率最高甚至是最有用的。他的廣泛應用已經超過30年，包括機器人導航，控制，傳感器數據融合甚至在軍事方面的雷達系統以及導彈追蹤等等。近年來更被應用于計算機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等。

　　卡爾曼濾波的算法

　　卡爾曼濾波器算法 ：

　　在這一部分，我們就來描述源于Dr Kalman 的卡爾曼濾波器。下面的描述，會涉及一些基本的概念知識，包括概率（Probability），隨機變量（Random Variable），高斯或正態分配（Gaussian Distribution）還有State-space Model等等。但對于卡爾曼濾波器的詳細證明，這里不能一一描述。

　　首先，我們先要引入一個離散控制過程的系統。該系統可用一個線性隨機微分方程（Linear Stochastic Difference equation）來描述：

　　X（k）=A X（k-1）+B U（k）+W（k）

　　再加上系統的測量值：

　　Z（k）=H X（k）+V（k）

　　上兩式子中，X（k）是k時刻的系統狀態，U（k）是k時刻對系統的控制量。A和B是系統參數，對于多模型系統，他們為矩陣。Z（k）是k時刻的測量值，H是測量系統的參數，對于多測量系統，H為矩陣。W（k）和V（k）分別表示過程和測量的噪聲。他們被假設成高斯白噪聲（White Gaussian Noise），他們的協方差（covariance）分別是Q，R（這里我們假設他們不隨系統狀態變化而變化）。

　　對于滿足上面的條件（線性隨機微分系統，過程和測量都是高斯白噪聲），卡爾曼濾波器是最優的信息處理器。下面我們結合他們的協方差來估算系統的最優化輸出（類似上一節那個溫度的例子）。

　　首先我們要利用系統的過程模型，來預測下一狀態的系統。假設現在的系統狀態是k，根據系統的模型，可以基于系統的上一狀態而預測出現在狀態：

　　X（k|k-1）=A X（k-1|k-1）+B U（k） ………。。 （1）

　　式（1）中，X（k|k-1）是利用上一狀態預測的結果，X（k-1|k-1）是上一狀態最優的結果，U（k）為現在狀態的控制量，如果沒有控制量，它可以為0。

　　到現在為止，我們的系統結果已經更新了，可是，對應于X（k|k-1）的協方差還沒更新。我們用P表示協方差（covariance）：

　　P（k|k-1）=A P（k-1|k-1） A’+Q ……… （2）

　　式（2）中，P（k|k-1）是X（k|k-1）對應的協方差，P（k-1|k-1）是X（k-1|k-1）對應的協方差，A’表示A的轉置矩陣，Q是系統過程的協方差。式子1，2就是卡爾曼濾波器5個公式當中的前兩個，也就是對系統的預測。

　　現在我們有了現在狀態的預測結果，然后我們再收集現在狀態的測量值。結合預測值和測量值，我們可以得到現在狀態（k）的最優化估算值X（k|k）：

　　X（k|k）= X（k|k-1）+Kg（k） （Z（k）-H X（k|k-1）） ……… （3）

　　其中Kg為卡爾曼增益（Kalman Gain）：

　　Kg（k）= P（k|k-1） H’ / （H P（k|k-1） H’ + R） ……… （4）

　　到現在為止，我們已經得到了k狀態下最優的估算值X（k|k）。但是為了要令卡爾曼濾波器不斷的運行下去直到系統過程結束，我們還要更新k狀態下X（k|k）的協方差：

　　P（k|k）=（I-Kg（k） H）P（k|k-1） ……… （5）

　　其中I 為1的矩陣，對于單模型單測量，I=1。當系統進入k+1狀態時，P（k|k）就是式子（2）的P（k-1|k-1）。這樣，算法就可以自回歸的運算下去。

　　卡爾曼濾波器的原理基本描述了，式子1，2，3，4和5就是他的5 個基本公式。根據這5個公式，可以很容易用計算機編程實現。

　　在上面的例子中，過程誤差和測量誤差設定為4是為了討論的方便。實際中，溫度的變化速度以及溫度計的測量誤差都沒有這么大。