Kalman Filter是一個(gè)高效的遞歸濾波器，它可以實(shí)現(xiàn)從一系列的噪聲測(cè)量中，估 計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)。廣泛應(yīng)用于包含Radar、計(jì)算機(jī)視覺(jué)在內(nèi)的等工程應(yīng)用領(lǐng)域，在控制理論和控制系統(tǒng)工程中也是一個(gè)非常重要的課題。連同線性均方規(guī)劃，卡爾曼濾波器可以用于解決LQG(Linear-quadratic-Gaussian control)問(wèn)題?？柭鼮V波器，線性均方歸化及線性均方高斯控制器，是大部分控制領(lǐng)域基礎(chǔ)難題的主要解決途徑。

目錄

■    1     應(yīng)用實(shí)例

■    2    命名和發(fā)展歷史

■    3    基本動(dòng)態(tài)系統(tǒng)模型

■    4    卡爾曼濾波器

            4.1    預(yù)測(cè)

            4.2    更新

            4.3    不變量

■    5    實(shí)例

■    6    推導(dǎo)

           6.1    后驗(yàn)估計(jì)協(xié)方差矩陣推導(dǎo)

           6.2    Kalman 增益推導(dǎo)

           6.3    后驗(yàn)誤差協(xié)方差矩陣簡(jiǎn)化

■    7    信息濾波

■    8  非線性濾波器

           8.1   擴(kuò)展Kalman 濾波

           8.2   Unscented Kalman filter

■    9   Kalman-Bucy濾波

■    10  應(yīng)用

■    11  參見(jiàn)

■    12  參考文獻(xiàn)

■    13  外部鏈接

■    1    應(yīng)用實(shí)例

一個(gè)簡(jiǎn)單的應(yīng)用是估計(jì)物體的位置和速度；簡(jiǎn)要描述如下：假設(shè)我們可以獲取一個(gè)物體的包含噪聲的一系列位置觀測(cè)數(shù)據(jù)，我們可以獲得此物體的精確速度和位置連續(xù)更新信息。

例如，對(duì)于雷達(dá)來(lái)說(shuō)，我們關(guān)心的是跟蹤目標(biāo)，而目標(biāo)的位置，速度，加速度的測(cè)量值是時(shí)刻含有誤差的，卡爾曼濾波器利用目標(biāo)的動(dòng)態(tài)信息，去掉噪聲影響，獲取目標(biāo)此刻好的位置估計(jì)(濾波)，將來(lái)位置估計(jì)(預(yù)測(cè))，也可以是過(guò)去位置估計(jì)的(插值或平滑)

■    2    命名和發(fā)展歷史

這個(gè)濾波器以它的發(fā)明者Rudolf.E.Kalman 而命名，但是在Kanlman之前，Thorvald Nicolai Thiele和Peter Swerling 已經(jīng)提出了類似的算法。Stanley Schmidt 首次實(shí)現(xiàn)了Kalman濾波器。在一次對(duì)NASA Ames Research Center訪問(wèn)中，卡爾曼發(fā)現(xiàn)他的方法對(duì)于解決阿波羅計(jì)劃的軌跡預(yù)測(cè)很有用，后來(lái)阿波羅飛船導(dǎo)航 電腦就使用了這種濾波器。這個(gè)濾波器可以追溯到Swerling(1958),Kalman(1960),Kalman和Bucy(1961)發(fā)表的論文。

這個(gè)濾波器有時(shí)叫做Stratonovich-Kalman-Bucy濾波器。因?yàn)楦鼮橐话愕姆蔷€性濾波器最初由Ruslan L.Stratonovich發(fā)明，而Stratonovich-Kalman-Bucy濾波器只是非線性濾波器的一個(gè)特例。事實(shí)上，1960年夏季，Kalman和Stratonovich在一個(gè)Moscow召開的會(huì)議中相遇，而作為非線性特例的線性濾波方程，早已經(jīng)由Stratonovich在此以前發(fā)表了。

 在控制領(lǐng)域，Kalman濾波被稱為線性二次型估計(jì)，目前,卡爾曼濾波已經(jīng)有很多不同的實(shí)現(xiàn)，有施密特?cái)U(kuò)展濾波器、信息濾波器以及一系列的Bierman和Thornton 發(fā)明的平方根濾波器等，而卡爾曼最初提出的形式現(xiàn)在稱為簡(jiǎn)單卡爾曼濾波器。也許最常見(jiàn)的卡爾曼濾波器應(yīng)用是鎖相環(huán)，它在收音機(jī)、計(jì)算機(jī)和幾乎全部視頻或通訊設(shè)備中廣泛存在。

 ■   3    基本動(dòng)態(tài)系統(tǒng)模型

Kalman濾波基于時(shí)域描述的線性動(dòng)態(tài)系統(tǒng)，它的模型是Markov Chain，而Markov Chain建立在一個(gè)被高斯噪聲干擾的線性算子之上。系統(tǒng)的狀態(tài)可以用一個(gè)元素為實(shí)數(shù)的向量表示。 隨著離散時(shí)間的增加，這個(gè)線性算子就會(huì)作用到當(dāng)前狀態(tài)之上，產(chǎn)生一個(gè)新的狀態(tài)，并且會(huì)帶入一定的噪聲，同時(shí)一些已知的控制信息也會(huì)加入。同時(shí)另外一個(gè)受噪聲干擾的線性算子將產(chǎn)生這些隱含狀態(tài)的可見(jiàn)輸出。Kalman 濾波可以被看作為類似隱馬爾科夫模型，它們的顯著不同點(diǎn)在于：隱狀態(tài)變量的取值空間是一個(gè)連續(xù)的空間，而離散狀態(tài)空間則不是；另為，隱馬爾科夫模型可以描述下一個(gè)狀態(tài)的一個(gè)任意分布，這也與應(yīng)用于Kalman濾波器中的高斯噪聲模型相反。Kalman濾波器方程和隱馬爾科夫方程之間有很大的二重性，關(guān)于Kalman 濾波方程和隱馬爾科夫方程之間二重性參看Roweis and Ghahramani(1999)[4]。              

 為了從一系列的噪聲觀測(cè)中，應(yīng)用Kalman濾波估計(jì)觀測(cè)過(guò)程的內(nèi)部狀態(tài)。我們必須把這個(gè)過(guò)程在Kalman 濾波器的框架下建立模型， 這就意味著，對(duì)于

每一步k  我們要定義矩陣 、 、 、 、 如下：

Kalman Filter 假設(shè)k 時(shí)刻的真實(shí)狀態(tài)是從k-1時(shí)刻演化而來(lái)，符合下式

這里

■ 是作用在前一狀態(tài)的狀態(tài)轉(zhuǎn)移模型(狀態(tài)轉(zhuǎn)移矩陣)

■  是作用在控制向量 上的控制輸入模型(輸入輸出矩陣)

■ 是過(guò)程噪聲，假設(shè)是均值為0的白噪聲，協(xié)方差為 則：

在k時(shí)刻，假設(shè)真實(shí)狀態(tài) 的觀測(cè)， 滿足如下公式：

其中 是觀測(cè)模型(觀測(cè)矩陣)，它把真實(shí)狀態(tài)映射到觀測(cè)空間， 是觀測(cè)噪聲，假設(shè)它是均值是0，方差是 的高斯白噪聲：

Kalman Filter基本動(dòng)態(tài)系統(tǒng)模型如圖(1)所示，圓圈代表向量，方塊代表矩陣，星號(hào)代表高斯噪聲，其協(xié)方差在右下方標(biāo)出。

初始狀態(tài)以及每一時(shí)刻的噪聲向量{x0, w1, ..., wk, v1 ... vk} 都為認(rèn)為是互相獨(dú)立的。實(shí)際中，真實(shí)世界中動(dòng)態(tài)系統(tǒng)并不是嚴(yán)格的符合此模型。但是Kalman模型是設(shè)計(jì)在噪聲過(guò)程工作的，一個(gè)近似的符合已經(jīng)可以使這個(gè)濾波器非常有用了，更多復(fù)雜模型關(guān)于Kalman Filter模型的變種，將在下述中討論：

                                                                     圖(1)

■    4    卡爾曼濾波器

Kalman Filter 是一個(gè)遞歸的估計(jì)，即只要獲知上一時(shí)刻的狀態(tài)估計(jì)和當(dāng)前狀態(tài)的觀測(cè)就可以計(jì)算出當(dāng)前狀態(tài)的估計(jì)，不同于其他的估計(jì)技術(shù)，Kalman 濾波器不需要觀測(cè)或/和估計(jì)的歷史記錄，Kalman Filter 是一個(gè)純粹的時(shí)域?yàn)V波器，而不像低通濾波器等頻域?yàn)V波器那樣，需要在頻域中設(shè)計(jì)，然后轉(zhuǎn)換到時(shí)域中應(yīng)用。

下面，代表已知從m到n-1包括m時(shí)刻的觀測(cè)在n時(shí)刻的估計(jì)值

卡爾曼濾波器的狀態(tài)由以下兩個(gè)變量表示：

      ■ 已知k時(shí)刻以前時(shí)刻觀測(cè)值，k時(shí)刻的狀態(tài)估計(jì)值

      ■ 誤差協(xié)方差矩陣，度量狀態(tài)估計(jì)的精度程度

Kalman 濾波包括兩個(gè)階段：預(yù)測(cè)和更新；在估計(jì)階段，濾波器應(yīng)用上一狀態(tài)的估計(jì)做出對(duì)當(dāng)前狀態(tài)的估計(jì)。在更新階段，濾波器利用在當(dāng)前狀態(tài)的觀測(cè)值優(yōu)化預(yù)測(cè)階段的預(yù)測(cè)值，以獲的一個(gè)更精確的當(dāng)前狀態(tài)的估計(jì)。

4.1    預(yù)測(cè)

狀態(tài)預(yù)測(cè)：$$

■

估計(jì)協(xié)方差預(yù)測(cè)：

■

4.2    更新

新息或測(cè)量余量

■

新息協(xié)方差

■

Kalman 增益

■

狀態(tài)估計(jì)更新

■

狀態(tài)協(xié)方差更新

■

使用上述公式計(jì)算 僅在最優(yōu)卡爾曼增益的時(shí)候有效。使用其他增益公式要復(fù)雜一些，看見(jiàn)推導(dǎo)

4.3    不變量

如果模型準(zhǔn)確， 和 值將準(zhǔn)確反映最初狀態(tài)的分布，那么下面所有不變量保持不變，所有估計(jì)的誤差均值為0：

■

■

這里 表示 的期望，而協(xié)方差矩陣則反映的估計(jì)的協(xié)方差

■

■

■

■ 5  實(shí)例

考慮在一個(gè)無(wú)摩擦、無(wú)限長(zhǎng)的直軌道上的一輛小車，它的初始位置在0點(diǎn)，但是它會(huì)隨機(jī)的受到?jīng)_擊作用，我們每隔測(cè)量一次小車的位置，但是這些測(cè)量數(shù)據(jù)不是很精確。我們想建立一個(gè)關(guān)于小車位置和速度的模型，這里我們描述如何建立這個(gè)模型，以及從這個(gè)模型出發(fā)如何推導(dǎo)出Kalman 濾波器。

因?yàn)樾≤嚊](méi)有控制輸入，我們可以忽略和。由于F，H，R和Q全是恒值，我們可以忽略時(shí)間下標(biāo)。

小車的位置和速度用線性空間可以描述如下：

這里表示速度，也就是位置對(duì)時(shí)間的微分。

我們假設(shè)在時(shí)間間隔k-1和k之間，小車受到一個(gè)恒定的沖擊 ， 服從均值為0，方差為的正態(tài)分布，根據(jù)Newton 動(dòng)力學(xué)方程，可得到：

 其中  

 ，

我們發(fā)現(xiàn)：

在每一時(shí)刻,我們獲取真實(shí)位置的.我們假設(shè)噪聲服噪聲干擾測(cè)量，假設(shè)測(cè)量噪聲服從均值為0，標(biāo)準(zhǔn)差為正態(tài)分布。

  其中 H= [1  0]，

我們可以得到足夠精度的初始狀態(tài)數(shù)據(jù)，所以我們可以初始化 ，  

如果初始位置和速度不是精確的知道，那么協(xié)方差矩陣應(yīng)該初始化為一個(gè)對(duì)角線元素B為適當(dāng)大小的矩陣如下：

這樣與模型中已有信息相比，濾波器更趨向于使用首次的測(cè)量數(shù)據(jù)信息。

■  6   推導(dǎo)

 6.1  后驗(yàn)估計(jì)協(xié)方差矩陣推導(dǎo)

首先開始不變量后驗(yàn)估計(jì)協(xié)方差矩陣的推導(dǎo)：  帶入 定義，可得 ，代入 可得 代入 可得 整理誤差向量可得， 由于誤差向量 與其他不相關(guān)，所以 由協(xié)方差矩陣性質(zhì)則 ：使用不變量Pk|k-1以及Rk的定義這一項(xiàng)可以寫作 ，此公式(Joseph form)對(duì)任意增益Kk的都成立，如果Kk最優(yōu)卡爾曼增益，則可以進(jìn)一步簡(jiǎn)化，見(jiàn)下文。

6.2  Kalman 增益推導(dǎo)

Kalman 濾波器是一個(gè)最小均方誤差估計(jì)器，先驗(yàn)狀態(tài)誤差估計(jì)可表示為 我們最小化這個(gè)矢量幅度平方的期望值 ，這等價(jià)于最小化后驗(yàn)估計(jì)協(xié)方差矩陣 的跡，通過(guò)展開合并 公式，可得

當(dāng)矩陣導(dǎo)數(shù)為0時(shí)，矩陣的跡取最小值，

從這個(gè)式子解出Kalman增益

這個(gè)增益就是最優(yōu)Kalman增益，應(yīng)用它可以得到最小均方誤差。

6.3    后驗(yàn)誤差協(xié)方差矩陣簡(jiǎn)化

當(dāng)應(yīng)用上述最優(yōu)Kalman增益時(shí)，后驗(yàn)誤差協(xié)方差可以得到簡(jiǎn)化，在最優(yōu)Kalman增益兩邊同時(shí)乘以 ，可得 ，參見(jiàn)后驗(yàn)誤差協(xié)方差公式展開 ，帶入上式，可得： 這個(gè)公式的計(jì)算比較簡(jiǎn)單，所以實(shí)際中總是使用這個(gè)公式，但是需注意這公式僅在最優(yōu)卡爾曼增益時(shí)它才成立。如果算術(shù)精度總是很低而導(dǎo)致數(shù)值穩(wěn)定性出現(xiàn)問(wèn)題，或者特意使用非最優(yōu)卡爾曼增益，那么就不能使用這個(gè)簡(jiǎn)化；必須使用上面導(dǎo)出的后驗(yàn)誤差協(xié)方差公式。

 ■    7    信息濾波

在信息濾波器(逆方差濾波器)中，協(xié)方差估計(jì)和狀態(tài)估計(jì)將會(huì)被信息矩陣和信息向量所取代，它們的定義如下：

類似的預(yù)測(cè)協(xié)方差和預(yù)測(cè)狀態(tài)也有等價(jià)的信息形式，定義如下：

同樣測(cè)量協(xié)方差和測(cè)量向量定義為：

信息更新現(xiàn)在變成一個(gè)加和形式：

信息濾波器的主要優(yōu)點(diǎn)在于N和測(cè)量數(shù)據(jù)都可以用于濾波，簡(jiǎn)單的通過(guò)信息矩陣和信息向量的加和。

為了預(yù)測(cè)信息濾波器，信息矩陣和信息向量必須變換到它們的等價(jià)狀態(tài)空間，或者應(yīng)用下述信息空間更新：

這里F和Ｑ必須可逆。

■ 8  非線性濾波器

    8.1 擴(kuò)展Kalman 濾波

　　估計(jì)過(guò)程

如以上所述，卡爾曼濾波器估計(jì)一個(gè)線性隨機(jī)差分方程描述的離散時(shí)間過(guò)程的狀態(tài)變量，但是如果被估計(jì)的過(guò)程和(或)觀測(cè)變量與過(guò)程的關(guān)系不時(shí)線性關(guān)系。那該如何處理呢？一些很有趣和成功的Kalman濾波器應(yīng)用就是處理這些情況的。將期望和方差線性化的卡爾曼濾波器稱作擴(kuò)展卡爾曼濾波器（Extended Kalman Filter），簡(jiǎn)稱EKF。

同泰勒級(jí)數(shù)類似，面對(duì)非線性關(guān)系時(shí)，我們可以通過(guò)求過(guò)程和量測(cè)方程的偏導(dǎo)來(lái)線性化并計(jì)算當(dāng)前估計(jì)，為了實(shí)現(xiàn)這個(gè)目的，我們必須修改上面的一些描述，我們假設(shè)過(guò)程仍具有狀態(tài)向量，但其狀態(tài)方程已變?yōu)榉蔷€性隨機(jī)差分方程的形式。

觀測(cè)變量 為：

這里隨機(jī)變量 和 分別為過(guò)程噪聲和觀測(cè)噪聲。差分方程式(1.1)中的非線性函數(shù)f 將過(guò)去k-1時(shí)刻狀態(tài)與現(xiàn)在k時(shí)刻狀態(tài)聯(lián)系起來(lái)。在測(cè)量方程(2.2)中，輸入函數(shù)uk和零均值過(guò)程噪聲wk是它的參數(shù)。非線性函數(shù)h 反映了狀態(tài)變量xk 和觀測(cè)變量zk 的關(guān)系。

實(shí)際中我們并不知道每一時(shí)刻噪聲wk 和vk 各自真實(shí)值，但是我們可以在假設(shè)他們不存在的前提下，近似估計(jì)狀態(tài)向量和測(cè)量向量：

這里 是相對(duì)于前一時(shí)刻k的后驗(yàn)狀態(tài)估計(jì)。

　　有一點(diǎn)非常重要，那就是擴(kuò)展卡爾曼濾波器的一個(gè)基本缺陷：離散隨機(jī)變量的分布(或連續(xù)隨機(jī)變量的密度)在經(jīng)過(guò)非線性系統(tǒng)轉(zhuǎn)化后不再是正態(tài)的了。擴(kuò)展卡爾曼濾波器其實(shí)就是一個(gè)通過(guò)線性化而達(dá)到漸進(jìn)最優(yōu)貝葉斯決策的特殊狀態(tài)估計(jì)器。[Julier96]中描述了一項(xiàng)有趣的研究，Julier設(shè)計(jì)了擴(kuò)展卡爾曼濾波器的一種變體，使得通過(guò)非線性轉(zhuǎn)換后的隨機(jī)變量仍具有正態(tài)分布特性。

濾波器的計(jì)算原型

   為了估計(jì)一個(gè)具有非線性差分和量測(cè)關(guān)系的過(guò)程，我們先給出式1.3和式1.4的一個(gè)新的線性化表示：

 其中：

　　xk 和zk是狀態(tài)向量和觀測(cè)向量的真值；

　　 和 來(lái)自1.3式和1.4式，是狀態(tài)向量和觀測(cè)向量的近似值；

　  是k 時(shí)刻狀態(tài)向量的后驗(yàn)估計(jì)；

　   隨機(jī)變量wk 和vk 表示過(guò)程激勵(lì)噪聲和觀測(cè)噪聲。

       A是f 對(duì)x的偏導(dǎo)的雅可比矩陣：

　   W 是f 對(duì)w 的偏導(dǎo)的雅可比矩陣：

       H 是h 對(duì)x 的偏導(dǎo)的雅可比矩陣：

      V 是h 對(duì)v 的偏導(dǎo)的雅可比矩陣：

摘自：http://internetbuff.blog.163.com/blog/static/94251107200917113038144/