卡爾曼濾波原理及算法

　　卡爾曼濾波的工作原理

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

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

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

　　卡爾曼濾波的算法

　　卡爾曼濾波器算法 ：

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

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

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

　　再加上系統(tǒng)的測量值：

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

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

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

　　首先我們要利用系統(tǒng)的過程模型，來預(yù)測下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是k，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測出現(xiàn)在狀態(tài)：

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

　　式（1）中，X（k|k-1）是利用上一狀態(tài)預(yù)測的結(jié)果，X（k-1|k-1）是上一狀態(tài)最優(yōu)的結(jié)果，U（k）為現(xiàn)在狀態(tài)的控制量，如果沒有控制量，它可以為0。

　　到現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了，可是，對應(yīng)于X（k|k-1）的協(xié)方差還沒更新。我們用P表示協(xié)方差（covariance）：

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

　　式（2）中，P（k|k-1）是X（k|k-1）對應(yīng)的協(xié)方差，P（k-1|k-1）是X（k-1|k-1）對應(yīng)的協(xié)方差，A’表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過程的協(xié)方差。式子1，2就是卡爾曼濾波器5個(gè)公式當(dāng)中的前兩個(gè)，也就是對系統(tǒng)的預(yù)測。

　　現(xiàn)在我們有了現(xiàn)在狀態(tài)的預(yù)測結(jié)果，然后我們再收集現(xiàn)在狀態(tài)的測量值。結(jié)合預(yù)測值和測量值，我們可以得到現(xiàn)在狀態(tài)（k）的最優(yōu)化估算值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）

　　到現(xiàn)在為止，我們已經(jīng)得到了k狀態(tài)下最優(yōu)的估算值X（k|k）。但是為了要令卡爾曼濾波器不斷的運(yùn)行下去直到系統(tǒng)過程結(jié)束，我們還要更新k狀態(tài)下X（k|k）的協(xié)方差：

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

　　其中I 為1的矩陣，對于單模型單測量，I=1。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí)，P（k|k）就是式子（2）的P（k-1|k-1）。這樣，算法就可以自回歸的運(yùn)算下去。

　　卡爾曼濾波器的原理基本描述了，式子1，2，3，4和5就是他的5 個(gè)基本公式。根據(jù)這5個(gè)公式，可以很容易用計(jì)算機(jī)編程實(shí)現(xiàn)。

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