1、卡爾曼濾波簡介及其算法實現(xiàn)代碼 卡爾曼濾波算法實現(xiàn)代碼（C, C+分別實現(xiàn)）卡爾曼濾波器簡介近來發(fā)現(xiàn)有些問題很多人都很感興趣。所以在這里希望能盡自己能力跟大家討論一些 力所能及的算法?，F(xiàn)在先討論一下卡爾曼濾波器，如果時間和能力允許，我還希望能夠寫 寫其他的算法，例如遺傳算法，傅立葉變換，數(shù)字濾波，神經網絡，圖像處理等等。因為這里不能寫復雜的數(shù)學公式，所以也只能形象的描述。希望如果哪位是這方面的專 家，歡迎討論更正?？柭鼮V波器 -Kalman Filter1.什么是卡爾曼濾波器跟其他著名的理論（例如傅立 而跟他們不同的是，他是個現(xiàn)(What is the Kalma n Filter? )在學

2、習卡爾曼濾波器之前，首先看看為什么叫“卡爾曼”。 葉變換，泰勒級數(shù)等等）一樣，卡爾曼也是一個人的名字, 代人！卡爾曼全名 Rudolf Emil Kalman ，匈牙利數(shù)學家，1930年岀生于匈牙利首都布達佩 斯。1953，1954年于麻省理工學院分別獲得電機工程學士及碩士學位。1957年于哥倫比亞大學獲得博士學位。我們現(xiàn)在要學習的卡爾曼濾波器，正是源于他的博士論文和1960 年發(fā)表的論文A New Approach to Lin ear Filteri ng and PredictionProblems（線性濾波與預測問題的新方法）。如果對這編論文有興趣，可以到這里的地址下載：http:/w

3、/welch/media/pdf/Kalma n1960.pdf。簡單來說，卡爾曼濾波器是一個“optimal recursive data process ingalgorithm （最優(yōu)化自回歸數(shù)據(jù)處理算法）”。對于解決很大部分的問題，他是最優(yōu)，效 率最高甚至是最有用的。他的廣泛應用已經超過30年，包括機器人導航，控制，傳感器 數(shù)據(jù)融合甚至在軍事方面的雷達系統(tǒng)以及導彈追蹤等等。近年來更被應用于計算機圖像處 理，例如頭臉識別，圖像分割，圖像邊緣檢測等等。2 卡爾曼濾波器的介紹(In troduct ion to the Kalma n Filter)為了可以更加容易

4、的理解卡爾曼濾波器，這里會應用形象的描述方法來講解，而不是像大多數(shù)參考書那樣羅列一大堆的數(shù)學公式和數(shù)學符號。但是，他的5條公式是其核心內容。結合現(xiàn)代的計算機，其實卡爾曼的程序相當?shù)暮唵?，只要你理解了他的?條公式。在介紹他的5條公式之前，先讓我們來根據(jù)下面的例子一步一步的探索假設我們要研究的對象是一個房間的溫度。根據(jù)你的經驗判斷，這個房間的溫度是恒定 的，也就是下一分鐘的溫度等于現(xiàn)在這一分鐘的溫度（假設我們用一分鐘來做時間單 位）。假設你對你的經驗不是100%勺相信，可能會有上下偏差幾度。我們把這些偏差看成是高斯白噪聲（ White Gaussian Noise ），也就是這些偏差跟前后時間是

5、沒有關系 的而且符合高斯分配（Gaussia n Distributio n ）。另外，我們在房間里放一個溫度 計，但是這個溫度計也不準確的，測量值會比實際值偏差。我們也把這些偏差看成是高斯 白噪聲。好了，現(xiàn)在對于某一分鐘我們有兩個有關于該房間的溫度值：你根據(jù)經驗的預測值（系統(tǒng) 的預測值）和溫度計的值（測量值）。下面我們要用這兩個值結合他們各自的噪聲來估算 出房間的實際溫度值。假如我們要估算 k時刻的是實際溫度值。首先你要根據(jù)k-1時刻的溫度值，來預測k時刻的溫度。因為你相信溫度是恒定的，所以你會得到k時刻的溫度預測值是跟k-1時刻一樣的，假設是 23度，同時該值的高斯噪聲的偏差是5度（5是這

6、樣得到的：如果 k-1時刻估算岀的最優(yōu)溫度值的偏差是3,你對自己預測的不確定度是4度，他們平方相加再開方，就是5）。然后，你從溫度計那里得到了k時刻的溫度值，假設是 25度，同時該值的偏差是4度。由于我們用于估算 k時刻的實際溫度有兩個溫度值，分別是 23度和25度。究竟實際溫 度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點，我們可以用他們的covarianee來判斷。因為 KgA2=5A2/(5A2+4A2，所以Kg=0.78,我們可以估算岀 k 時刻的實際溫度值是：23+0.78*(25-23=24.56 度?？梢钥磳?，因為溫度計的covarianee比較小(比較相信溫度計)，所以

7、估算岀的最優(yōu)溫度值偏向溫度計的值?，F(xiàn)在我們已經得到 k時刻的最優(yōu)溫度值了，下一步就是要進入k+1時刻，進行新的最優(yōu)估算。到現(xiàn)在為止，好像還沒看到什么自回歸的東西岀現(xiàn)。對了，在進入k+1時刻之前，我們還要算岀k時刻那個最優(yōu)值(24.56度)的偏差。算法如下：(1-Kg*5A2A0.5=2.35。這里的5就是上面的k時刻你預測的那個 23度溫度值的偏差，得 岀的2.35就是進入k+1時刻以后k時刻估算岀的最優(yōu)溫度值的偏差(對應于上面的3)。就是這樣，卡爾曼濾波器就不斷的把covarianee遞歸，從而估算岀最優(yōu)的溫度值。他運行的很快，而且它只保留了上一時刻的covarianee。上面的Kg,就是卡

8、爾曼增益(Kalman Gain )。他可以隨不同的時刻而改變他自己的值，是不是很神奇！F面就要言歸正傳，討論真正工程系統(tǒng)上的卡爾曼3.卡爾曼濾波器算法(The Kalma n Filter Algorithm)在這一部分，我們就來描述源于Dr Kalman的卡爾曼濾波器。下面的描述，會涉及一些基本的概念知識，包括概率(Probability )，隨即變量(Random Variable )，高斯或正態(tài)分配(Gaussian Distribution)還有State-space Model等等。但對于卡爾曼濾波器的詳細證明，這里不能一一描述。首先，我們先要引入一個離散控制過程的系統(tǒng)。該系統(tǒng)可用

9、一個線性隨機微分方程(Lin ear Stochastic Differe nee equati on)來描述：X(k=A X(k-1+B U(k+W(k再加上系統(tǒng)的測量值：Z(k=H X(k+V(k上兩式子中，X(k是k時刻的系統(tǒng)狀態(tài)， U(k是k時刻對系統(tǒng)的控制量。A和B是系統(tǒng)參數(shù)，對于多模型系統(tǒng)，他們?yōu)榫仃?。Z(k是k時刻的測量值，H是測量系統(tǒng)的參數(shù)，對于多測量系統(tǒng)，H為矩陣。W(k和V(k分別表示過程和測量的噪聲。他們被假設成高斯白噪 聲(White Gaussian Noise ，他們的covarianee 分別是Q R (這里我們假設他們 不隨系統(tǒng)狀態(tài)變化而變化)。對于滿足上面的條

10、件(線性隨機微分系統(tǒng)，過程和測量都是高斯白噪聲，卡爾曼濾波器是最 優(yōu)的信息處理器。下面我們來用他們結合他們的covaria nces 來估算系統(tǒng)的最優(yōu)化輸岀(類似上一節(jié)那個溫度的例子)。首先我們要利用系統(tǒng)的過程模型，來預測下一狀態(tài)的系統(tǒng)。假設現(xiàn)在的系統(tǒng)狀態(tài)是k,根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預測岀現(xiàn)在狀態(tài)：X(k|k-仁A X(k-1|k-1+B U(k，”. (1式(1中，X(k|k-1是利用上一狀態(tài)預測的結果，X(k-1|k-1 是上一狀態(tài)最優(yōu)的結果，U(k為現(xiàn)在狀態(tài)的控制量，如果沒有控制量，它可以為0。到現(xiàn)在為止，我們的系統(tǒng)結果已經更新了，可是，對應于X(k|k-1的covar

11、ianee還沒更新。我們用 P表示covarianee :P(k|k-仁A P(k-1|k-1 A' +Q，”(2式(2 中，P(k|k-1 是 X(k|k-1 對應的 covarianee，P(k-1|k-1 是 X(k-1|k-1 對 應的covarianee ，A 表示A的轉置矩陣， Q是系統(tǒng)過程的 covarianee。式子1，2 就是卡爾曼濾波器 5個公式當中的前兩個，也就是對系統(tǒng)的預測?，F(xiàn)在我們有了現(xiàn)在狀態(tài)的預測結果，然后我們再收集現(xiàn)在狀態(tài)的測量值。結合預測值和測 量值，我們可以得到現(xiàn)在狀態(tài)(k的最優(yōu)化估算值 X(k|k :其中Kg為卡爾曼增益(Kalman Gain :K

12、g(k= P(k|k- 1 H' / (H P(k|k -1 H' + R ,(4到現(xiàn)在為止，我們已經得到了 k狀態(tài)下最優(yōu)的估算值 X(k|k。但是為了要另卡爾曼濾波器 不斷的運行下去直到系統(tǒng)過程結束，我們還要更新k狀態(tài)下X(k|k的covarianee :P(k|k=( I-Kg(k H)P(k|k-1，”(5其中I為1的矩陣，對于單模型單測量，1=1。當系統(tǒng)進入k+1狀態(tài)時，P(k|k就是式子(2的P(k-1|k-1 。這樣，算法就可以自回歸的運算下去?？柭鼮V波器的原理基本描述了，式子1, 2，3, 4和5就是他的5個基本公式。根據(jù)這5個公式，可以很容易的實現(xiàn)計算機的程序

13、。F面，我會用程序舉一個實際運行的例子。4.簡單例子(A Simple Example )這里我們結合第二第三節(jié)，舉一個非常簡單的例子來說明卡爾曼濾波器的工作過程。所舉 的例子是進一步描述第二節(jié)的例子，而且還會配以程序模擬結果。根據(jù)第二節(jié)的描述，把房間看成一個系統(tǒng)，然后對這個系統(tǒng)建模。當然，我們見的模型不需要非常地精確。我們所知道的這個房間的溫度是跟前一時刻的溫度相同的，所以A=1沒有控制量，所以 U(k=0。因此得岀：X(k|k-1=X(k-1|k-1，”. (6式子(2)可以改成:X(k|k= X(k|k-1+Kg(k (Z(k-X(k|k-1，”(8Kg(k= P(k|k-1 / (P(

14、k|k-1 + R ，”(9P(k|k= ( 1-Kg(k)P(k|k-1，”(10現(xiàn)在我們模擬一組測量值作為輸入。假設房間的真實溫度為25度，我模擬了 200個測量值，這些測量值的平均值為 25度，但是加入了標準偏差為幾度的高斯白噪聲(在圖中為 藍線)。為了令卡爾曼濾波器開始工作，我們需要告訴卡爾曼兩個零時刻的初始值，是X(0|0和P(0|0。他們的值不用太在意，隨便給一個就可以了，因為隨著卡爾曼的工作，X會逐漸的收斂。但是對于 P, 般不要取0,因為這樣可能會令卡爾曼完全相信你給定的X(0|0是系統(tǒng)最優(yōu)的，從而使算法不能收斂。我選了X(0|0=1度，P(0|0=10。該系統(tǒng)的真實溫度為 2

15、5度，圖中用黑線表示。圖中紅線是卡爾曼濾波器輸岀的最優(yōu)化結 果(該結果在算法中設置了Q=1e-6, R=1e-1)。最佳線性濾波理論起源于 40年代美國科學家 Wie ner和前蘇聯(lián)科學家Ko貝MoropoB等人的研究工作，后人統(tǒng)稱為維納濾波理論。從理論上說，維納濾波的最大缺點是必須用到無限過去的數(shù)據(jù)，不適用于實時處理。為了克服這一缺點，60年代Kalma n把狀態(tài)空間模型引入濾波理論，并導出了一套遞推估計算法，后人稱之為卡爾曼濾波理論?？柭鼮V波是以最小均方誤差為估 計的最佳準則，來尋求一套遞推估計的算法，其基本思想是：采用信號與噪聲的狀態(tài)空間模型，利 用前一時刻地估計值和現(xiàn)時刻的觀測值來更

16、新對狀態(tài)變量的估計，求岀現(xiàn)時刻的估計值。它適合于 實時處理和計算機運算?，F(xiàn)設線性時變系統(tǒng)的離散狀態(tài)防城和觀測方程為:X(k = F(k, k- 1 X(k-1+T (k,k- 1 U(k-1Y(k = H(k X(k+N(k其中X(k和Y(k分別是k時刻的狀態(tài)矢量和觀測矢量F(k, k-1為狀態(tài)轉移矩陣U(k為k時刻動態(tài)噪聲T(k, k-1為系統(tǒng)控制矩陣H(k為k時刻觀測矩陣N(k為k時刻觀測噪聲則卡爾曼濾波的算法流程為:預估計 X(kA= F(k,k- 1 X(k-11. 計算預估計協(xié)方差矩陣C(kA=F(k,k- 1X C(kx F(k,k -1'+T(k,k-1x Q(kx T(

17、k,k -1'Q(k = U(k x U(k'2. 計算卡爾曼增益矩陣K(k = C(kA x H(k' x H(k x C(kA x H(k'+R(kF(-1R(k = N(k x N(k'3. 更新估計X(k=X(kA+K(k x Y(k -H(kx X(kJ4. 計算更新后估計協(xié)防差矩陣C(k = I- K(k x H(k x C(kA x I - K(k x H(k'+K(k x R(kx K(k'5. X(k+1 = X(kC(k+1 = C(k重復以上步驟Kalma n Filter科技2010-05-29 21:13:49閱

18、讀90評論0字號：大中小 訂閱Kalma n Filter是一個高效的遞歸濾波器，它可以實現(xiàn)從一系列的噪聲測量中，估 計動態(tài)系統(tǒng)的狀態(tài)。廣泛應用于包含 Radar、計算機視覺在內的等工程應用領域, 在控制理論和控制系統(tǒng)工程中也是一個非常重要的課題。連同線性均方規(guī)劃，卡爾 曼濾波器可以用于解決LQG(Li near-quadratic-Gaussian con trol問題。卡爾曼濾波 器，線性均方歸化及線性均方高斯控制器，是大部分控制領域基礎難題的主要解決 途徑。目錄1應用實例2命名和發(fā)展歷史3基本動態(tài)系統(tǒng)模型4卡爾曼濾波器4.1預測4.2更新4.3不變量5實例6推導6.1后驗估計協(xié)方差矩陣推

19、導6.2Kalman增益推導6.3后驗誤差協(xié)方差矩陣簡化7信息濾波 8非線性濾波器8.1擴展Kalman濾波8.2 Un see nted Kalma n filter 9 Kalma n-Bucy 濾波 10應用 11參見 12參考文獻 13外部鏈接 1應用實例一個簡單的應用是估計物體的位置和速度；簡要描述如下：假設我們可以獲取一個 物體的包含噪聲的一系列位置觀測數(shù)據(jù)，我們可以獲得此物體的精確速度和位置連 續(xù)更新信息。例如，對于雷達來說，我們關心的是跟蹤目標，而目標的位置，速度，加速度的測 量值是時刻含有誤差的，卡爾曼濾波器利用目標的動態(tài)信息，去掉噪聲影響，獲取 目標此刻好的位置估計(濾波，

20、將來位置估計(預測，也可以是過去位置估計的(插值 或平滑 2命名和發(fā)展歷史這個濾波器以它的發(fā)明者 而命名，但是在Kanlman之前， Thorvald Nicolai Thiele 和 Peter Swerling已經提出了類似的算法。Stanley Schmidt首次實現(xiàn)了 Kalman濾波器。在一次對 NASA Ames Research Cente訪問中，卡爾 曼發(fā)現(xiàn)他的方法對于解決阿波羅計劃的軌跡預測很有用，后來阿波羅飛船導航電腦就使用了這種濾波器。這個濾波器可以追溯到Swerling(1958,Kalman(1960,Kalman和 Bucy(1961 發(fā)表的論文。這個濾波器有時叫做

21、Stratonovich-Kalman-Bucy濾波器。因為更為一般的非線性濾 波器最初由 Ruslan L.Stratonovich發(fā)明，而 Stratonovich-Kalman-Bucy 濾波器只是非 線性濾波器的一個特例。事實上，1960年夏季，Kalman和Stratonovich在一個 Moscow召開的會議中相遇，而作為非線性特例的線性濾波方程，早已經由Strato no vich在此以前發(fā)表了。在控制領域，Kalman濾波被稱為線性二次型估計，目前，卡爾曼濾波已經有很多不 同的實現(xiàn)，有施密特擴展濾波器、信息濾波器以及一系列的Bierman和Thornton發(fā)明的平方根濾波器等，

22、而卡爾曼最初提出的形式現(xiàn)在稱為簡單卡爾曼濾波器。也許最常見的卡爾曼濾波器應用是鎖相環(huán)，它在收音機、計算機和幾乎全部視頻或通 訊設備中廣泛存在。 3基本動態(tài)系統(tǒng)模型Kalman濾波基于時域描述的線性動態(tài)系統(tǒng)，它的模型是Markov Chain，而MarkovChain建立在一個被高斯噪聲干擾的線性算子之上。系統(tǒng)的狀態(tài)可以用一個元素為 實數(shù)的向量表示。隨著離散時間的增加，這個線性算子就會作用到當前狀態(tài)之 上，產生一個新的狀態(tài)，并且會帶入一定的噪聲，同時一些已知的控制信息也會加 入。同時另外一個受噪聲干擾的線性算子將產生這些隱含狀態(tài)的可見輸出。Kalman濾波可以被看作為類似隱馬爾科夫模型，它們的顯

23、著不同點在于：隱狀態(tài) 變量的取值空間是一個連續(xù)的空間，而離散狀態(tài)空間則不是；另為，隱馬爾科夫模 型可以描述下一個狀態(tài)的一個任意分布，這也與應用于Kalman濾波器中的高斯噪聲模型相反。Kalman濾波器方程和隱馬爾科夫方程之間有很大的二重性，關于 Kalman濾波方程和隱馬爾科夫方程之間二重性參看Roweis andGhahrama ni（19994。為了從一系列的噪聲觀測中，應用 Kalman濾波估計觀測過程的內部狀態(tài)。我們必 須把這個過程在Kalman濾波器的框架下建立模型，這就意味著，對于每一步k我們要定義矩陣'、=、=如下：Kalman Filter假設k時刻的真實狀態(tài)是從k-

24、1時刻演化而來，符合下式= F血a + B他 +%這里（是作用在前一狀態(tài)的狀態(tài)轉移模型（狀態(tài)轉移矩陣是作用在控制向量 上的控制輸入模型（輸入輸出矩陣 '是過程噪聲，假設是均值為0的白噪聲，協(xié)方差為貝''在k時刻，假設真實狀態(tài)，（的觀測，'滿足如下公式：乙=盡玉+叫其中“是觀測模型（觀測矩陣，它把真實狀態(tài)映射到觀測空間，1:是觀測噪聲，假設它是均值是0,方差是' 的高斯白噪聲：- 1Kalman Filter基本動態(tài)系統(tǒng)模型如圖（1所示，圓圈代表向量，方塊代表矩陣，星號 代表高斯噪聲，其協(xié)方差在右下方標出。初始狀態(tài)以及每一時刻的噪聲向量xO, w1, .,

25、 wk, v1 . vk都為認為是互相獨立 的。實際中，真實世界中動態(tài)系統(tǒng)并不是嚴格的符合此模型。但是Kalman模型是設計在噪聲過程工作的，一個近似的符合已經可以使這個濾波器非常有用了，更多 復雜模型關于Kalman Filter模型的變種，將在下述中討論：* iMmwiiHir in «圖（1 4卡爾曼濾波器Kalma n Filter是一個遞歸的估計，即只要獲知上一時刻的狀態(tài)估計和當前狀態(tài)的觀 測就可以計算出當前狀態(tài)的估計，不同于其他的估計技術，Kalman濾波器不需要觀測或/和估計的歷史記錄，Kalma n Filter是一個純粹的時域濾波器，而不像低通 濾波器等頻域濾波器那

26、樣，需要在頻域中設計，然后轉換到時域中應用。下面，“代表已知從m到n-1包括m時刻的觀測在n時刻的估計值卡爾曼濾波器的狀態(tài)由以下兩個變量表示：A %已知k時刻以前時刻觀測值，k時刻的狀態(tài)估計值；川誤差協(xié)方差矩陣，度量狀態(tài)估計的精度程度Kalman濾波包括兩個階段：預測和更新；在估計階段，濾波器應用上一狀態(tài)的估 計做出對當前狀態(tài)的估計。在更新階段，濾波器利用在當前狀態(tài)的觀測值優(yōu)化預測 階段的預測值，以獲的一個更精確的當前狀態(tài)的估計。4.1預測狀態(tài)預測:.；r." i 1 1估計協(xié)方差預測：十-1".- -.|4.2更新新息或測量余量新息協(xié)方差Kalma n增益狀態(tài)估計更新 r

27、 r r：. i-'J，狀態(tài)協(xié)方差更新 r r I丄-二.-'丄！-' Ll :使用上述公式計算小僅在最優(yōu)卡爾曼增益的時候有效。使用其他增益公式要復雜 一些，看見推導4.3不變量如果模型準確，九和 值將準確反映最初狀態(tài)的分布，那么下面所有不變量保 持不變，所有估計的誤差均值為 0： . . . ；：.- 二 I .< 1 r .這里J -表示-的期望，而協(xié)方差矩陣則反映的估計的協(xié)方差打 I：.W；、.；.N'. 1 I ； b.；5實例考慮在一個無摩擦、無限長的直軌道上的一輛小車，它的初始位置在0點，但是它會隨機的受到沖擊作用，我們每隔測量一次小車的位置，

28、但是這些測量數(shù)據(jù)不 是很精確。我們想建立一個關于小車位置和速度的模型，這里我們描述如何建立這 個模型，以及從這個模型出發(fā)如何推導出Kalman濾波器。因為小車沒有控制輸入，我們可以忽略=和由于F，H，R和Q全是恒值，我們可以忽略時間下標。小車的位置和速度用線性空間可以描述如下：這里'表示速度，也就是位置對時間的微分 我們假設在時間間隔k-1和k之間，小車受到一個恒定的沖擊服從均值為0,方差為；的正態(tài)分布，根據(jù)Newton動力學方程，可得到：其中1我們發(fā)現(xiàn):在每一時刻，我們獲取真實位置的我們假設噪聲服噪聲干擾測量，假設測量噪聲服 從均值為0，標準差為；正態(tài)分布。其中 H= 1 0，R =

29、 Ev = 我們可以得到足夠精度的初始狀態(tài)數(shù)據(jù)，所以我們可以初始化 ro q_時0 0如果初始位置和速度不是精確的知道，那么協(xié)方差矩陣應該初始化為一個對角線元 素B為適當大小的矩陣如下：0B這樣與模型中已有信息相比，濾波器更趨向于使用首次的測量數(shù)據(jù)信息 6推導 6.1后驗估計協(xié)方差矩陣推導首先開始不變量后驗估計協(xié)方差矩陣5：的推導：5宀"7 加；帶入，定義，可得 烏嚴匚曲（觀-（為“+ £A），代入 可得' - -/：；：.，: '| i : 11 代入二.可得斤u冃亡 WH - r屮扌十'+曲I心+叫整理誤差向量可得八，；" '

30、''；|； j u 由于誤差向量'與其他不相關，所以竊二川遼乩：飛 飛；由協(xié)方差矩陣性質則 mm已 叫譏d八怎5.曲：使用不變量pk|k-i以及Rk的定義這 一項可以寫作一'，此公式（Joseph form對任意增益Kk的都成立，如果Kk最優(yōu)卡爾曼增益，則可以進一步簡化，見下文。6.2 Kalman增益推導Kalman濾波器是一個最小均方誤差估計器，先驗狀態(tài)誤差估計可表示為；我 們最小化這個矢量幅度平方的期望值，這等價于最小化后驗估計協(xié)方 差矩陣的跡，通過展開合并公式，可得 補 P" 叩也叫盡魔+氐陸乩圮+聞肉=社】-K0Z -張狀蛭+隔戰(zhàn)=-2（比場

31、射y + 2冷瓦=0當矩陣導數(shù)為0時，矩陣的跡取最小值,從這個式子解出Kalman增益'K嚴洛H恣 這個增益就是最優(yōu)Kalman增益，應用它可以得到最小均方誤差6.3后驗誤差協(xié)方差矩陣簡化當應用上述最優(yōu)Kalman增益時，后驗誤差協(xié)方差可以得到簡化，在最優(yōu) Kalman 增益兩邊同時乘以二；：，可得-，參見后驗誤差協(xié)方差公式展開鳥屯=爲3 - 禺社1 _ 垃加1円Jt +丄疋i 帶入上式 可得：這個公式的計算比較簡單，所以實際中總是 使用這個公式，但是需注意這公式僅在最優(yōu)卡爾曼增益時它才成立。如果算術精度 總是很低而導致數(shù)值穩(wěn)定性出現(xiàn)問題，或者特意使用非最優(yōu)卡爾曼增益，那么就不 能使用

32、這個簡化；必須使用上面導出的后驗誤差協(xié)方差公式。 7信息濾波在信息濾波器（逆方差濾波器中，協(xié)方差估計和狀態(tài)估計將會被信息矩陣和信息向 量所取代，它們的定義如下：抵=擴類似的預測協(xié)方差和預測狀態(tài)也有等價的信息形式，定義如下:抵亠1二罰9L同樣測量協(xié)方差和測量向量定義為:信息更新現(xiàn)在變成一個加和形式:y城=幾I十2LS信息濾波器的主要優(yōu)點在于N和測量數(shù)據(jù)都可以用于濾波，簡單的通過信息矩陣 和信息向量的加和。為了預測信息濾波器，信息矩陣和信息向量必須變換到它們的等價狀態(tài)空間，或者 應用下述信息空間更新：抵廠九隅盤 + G姣煤夕畑 1 二 竝 if y-i這里F和Q必須可逆。8非線性濾波器8.1擴展K

33、alman濾波估計過程如以上所述，卡爾曼濾波器估計一個線性隨機差分方程描述的離散時間過程的狀態(tài) 變量'"：1"，但是如果被估計的過程和（或觀測變量與過程的關系不時線性關系。 那該如何處理呢？ 一些很有趣和成功的Kalman濾波器應用就是處理這些情況的。將期望和方差線性化的卡爾曼濾波器稱作擴展卡爾曼濾波器（Exte nded KalmanFilter），簡稱 EKF。同泰勒級數(shù)類似，面對非線性關系時，我們可以通過求過程和量測方程的偏導來線 性化并計算當前估計，為了實現(xiàn)這個目的，我們必須修改上面的一些描述，我們假 設過程仍具有狀態(tài)向量二廠"，但其狀態(tài)方程已變?yōu)?/p>

34、非線性隨機差分方程的形式。觀測變量亠為: 這里隨機變量：；和二分別為過程噪聲和觀測噪聲。差分方程式（1.1中的非線性函 數(shù)f將過去k-1時刻狀態(tài)與現(xiàn)在k時刻狀態(tài)聯(lián)系起來。在測量方程（2.2中，輸入函 數(shù)uk和零均值過程噪聲wk是它的參數(shù)。非線性函數(shù)h反映了狀態(tài)變量xk和觀測 變量zk的關系。實際中我們并不知道每一時刻噪聲 wk和vk各自真實值，但是我們可以在假設他 們不存在的前提下，近似估計狀態(tài)向量和測量向量：這里,是相對于前一時刻k的后驗狀態(tài)估計。有一點非常重要，那就是擴展卡爾曼濾波器的一個基本缺陷：離散隨機變量的 分布（或連續(xù)隨機變量的密度在經過非線性系統(tǒng)轉化后不再是正態(tài)的了。擴展卡爾 曼

35、濾波器其實就是一個通過線性化而達到漸進最優(yōu)貝葉斯決策的特殊狀態(tài)估計器。 Julier96中描述了一項有趣的研究，Julier設計了擴展卡爾曼濾波器的一種變體， 使得通過非線性轉換后的隨機變量仍具有正態(tài)分布特性。濾波器的計算原型為了估計一個具有非線性差分和量測關系的過程，我們先給出式1.3和式1.4的一個新的線性化表示：叫"總+地和-社L）+旳T比出心+刃（心-壬）+叫其中：xk和zk是狀態(tài)向量和觀測向量的真值；和二來自1.3式和1.4式，是狀態(tài)向量和觀測向量的近似值；，c是k時刻狀態(tài)向量的后驗估計；隨機變量wk和vk表示過程激勵噪聲和觀測噪聲。A是f對x的偏導的雅可比矩陣：W是f對w

36、的偏導的雅可比矩陣:H是h對x的偏導的雅可比矩陣:V是h對v的偏導的雅可比矩陣:3辭Z最佳線性濾波理論起源于40年代美國科學家 Wiener和前蘇聯(lián)科學家K 0孔MOrOpOB等人的研究工作，后人統(tǒng)稱為維納濾波理論。從理論上 說，維納濾波的最大缺點是必須用到無限過去的數(shù)據(jù)，不適用于實時處理。 為了克服這一缺點，60年代Kalman把狀態(tài)空間模型引入濾波理論，并導出 了一套遞推估計算法，后人稱之為卡爾曼濾波理論?？柭鼮V波是以最小均 方誤差為估計的最佳準則，來尋求一套遞推估計的算法，其基本思想是：采 用信號與噪聲的狀態(tài)空間模型，利用前一時刻地估計值和現(xiàn)時刻的觀測值來 更新對狀態(tài)變量的估計，求出現(xiàn)

37、時刻的估計值。它適合于實時處理和計算機 運算?，F(xiàn)設線性時變系統(tǒng)的離散狀態(tài)防城和觀測方程為：X(k = F(k, k- 1 X(k-1+T (k,k- 1 U(k-1Y(k = H(k X(k+N(k其中X(k和Y(k分別是k時刻的狀態(tài)矢量和觀測矢量F(k,k-1 為狀態(tài)轉移矩陣U(k為k時刻動態(tài)噪聲T(k, k-1為系統(tǒng)控制矩陣H(k為k時刻觀測矩陣N(k為k時刻觀測噪聲則卡爾曼濾波的算法流程為:1. 預估計 X(kA= F(k,k-1 X(k-12. 計算預估計協(xié)方差矩陣C(kA=F(k,k-1xC(k xF(k,k-1'+T(k,k-1Q(k x T(k,k-1'Q(k =

38、 U(kxU(k'3. 計算卡爾曼增益矩陣K(k = C(kA xH(k' XH(k xC(kA x H(k'+R(kF(-1R(k = N(kXN(k'4. 更新估計X(k=X(kA+K(k x Y(k-H(kxX(kA5. 計算更新后估計協(xié)防差矩陣C(k = I-K(kxH(k xC(kA x I-K(kxH(k'+K(kXR(k xK(k'6. X(k+1 = X(kC(k+1 = C(k重復以上步驟 int i,j,kk,ii,l,jj,js;Ldouble *e,*a,*b;1e=malloc(m*m* sizeof(double;l=

39、m;Lif (l1a=malloc(l*l* sizeof ( double ;b=malloc(l*l* sizeof ( double ;1for (i=0; i<=n-1; i+Lfor (j=0; jv=n-1; j+旺 ii=i*l+j; aii=0.0;1for (kk=0; kk<=n-1; kk+aii=aii+pi* n+kk*f n+kk;卜nIufor (i=0; i<=n-1; i+1for (j=0; jv=n-1; j+餉 ii=i*n+j; pii=qii;Ifor (kk=0; kk<=n-1; kk+Lpii=pii+fn+kk*akk

40、*l+j;卜1for (ii=2; ii<=k; ii+罪匚 for (i=0; i<=n-1; i+for (j=0; j<=m-1; j+滬 i*l+j; ajj=0.0;Lfor (kk=0; kk<=n-1; kk+1ajj=ajj+pi* n+kk*hj*in+kk;卜Lfor (i=0; i<=m-1; i+1for (j=0; j<=m-1; j+匚滬 i*m+j； ejj=rjj;Lfor (kk=0; kk<=n-1; kk+1ejj=ejj+hi* n+kk*akk*l+j;FLjs=ri nv(e,m;if (js=0誦 free

41、(e; free(a; free(b; return (js;1for (i=0; i<=n-1; i+Lfor (j=0; j<=m-1; j+-滬i*m+j; gjj=0.0;rfor (kk=0; kk<=m-1; kk+igjj=gjj+ai*l+kk*e j*m+kk;卜rfor (i=0; i<=n-1; i+滬(ii-1* n+i; xjj=0.0;ifor (j=0; jv=n-1; j+Ixjj=xjj+fi* n +j*x(ii-2* n+j;上ifor (i=0; i<=m-1; i+滬 i*l; bjj=y(ii-1*m+i;Lfor (j

42、=0; jv=n-1; j+1bjj=bjj-hi* n+j*x(ii-1* n+j;卜TL卜JJ 誦ILL噩Lfor (i=0; i<=n-1; i+- 滬(ii-1*n+i;for (j=0; jv=m-1; j+xjj=xjj+g i *m+j*bl;if (ii二 for (i=0; i<=n-1; i+for (j=0; jv=n-1; j+ 匚 jj=i*l+j; ajj=0.0;for (kk=0; kk<=m-1; kk+ ajj=ajj-g i *m+kk*hkk* n+j; if (i=j ajj=1.0+ajj;for (i=0; i<=n-1;

43、i+for (j=0; jv=n-1; j+- jj=i*l+j; bjj=0.0;for (kk=0; kk<=n-1; kk+bjj=bjj+ai*l+kk*pkk* n+j;for (i=0; i<=n-1; i+for (j=0; jv=n-1; j+- jj=i*l+j; ajj=0.0;for (kk=0; kk<=n-1; kk+ ajj=ajj+bi*l+kk*fj* n+kk;for (i=0; i<=n-1; i+ for (j=0; jv=n-1; j+_jj=i*n+j; pjj=qjj;for (kk=0; kk<=n-1; kk+ pj

44、j=pjj+fi* n+kk*al+kk;free(e; free(a; free(b;return (js;C+實現(xiàn)代碼如下：=kalma n.h=/ kalman.h: interface for the kalman class./ /#if !defined(AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2C0_INCL UDED_#defi ne AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2C0_INCLUDE D_#if _MSC_VER > 1000#pragma once#en

45、 dif / _MSC_VER > 1000#i nclude#i nclude "cv.h" class kalmanpublic :void init_kalman(int x, int xv, int y, int yv;CvKalma n* cvkalma n;CvMat* state;CvMat* process, no ise;CvMat* measureme nt;const CvMat* prediction;CvPoint2D32f get_predict(float x, float y;kalman(int x=0, int xv=0, int

46、y=0, int yv=0;/virtual kalma n(;#en dif / !defi ned(AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2C0_ _INCLUDED_kalma n. cpp#i nclude "kalma n.h"#i nclude/* tester de prin ter toutes les valeurs des vecteurs*/* tester de cha nger les matrices du no ises */* replace state by cvkalma n->

47、state_post ? */CvRa ndState rng;const double T = 0.1;kalman:kalman( int x, int xv, int y, int yvcvkalma n = cvCreateKalma n( 4, 4, 0 ;state = cvCreateMat( 4, 1, CV_32FC1 ;process_ noise = cvCreateMat( 4, 1, CV_32FC1 ;measureme nt = cvCreateMat( 4,1, CV_32FC1 ;int code = -1;/* create matrix data */co

48、n st float A = 1, T, 0, 0,0, 0,1, T,0, 0, 0, 1；con st float H = 1, 0, 0, 0,0, 0, 0, 0,0, 0, 1, 0,0, 0, 0, 0；con st float P = pow(320,2, pow(320,2/T, 0, 0,pow(320,2/T, pow(320,2/pow(T,2, 0, 0,0, 0, pow(240,2, pow(240,2/T,0, 0, pow(240,2/T, pow(240,2/pow (T,2；const float Q = pow (T ,3/3, pow(T,2/2, 0,

49、 0,pow (T,2/2, T, 0, 0,0, 0, pow(T,3/3, pow(T,2/2,0, 0, pow(T,2/2, T；con st float R = 1, 0, 0, 0,0, 0, 0, 0,0, 0,1,0,0, 0, 0, 0;cvRa ndln it( &rng, 0, 1, -1, CV_RAND_UNI ;cvZero( measureme nt ;cvRandSetRange( &rng, 0, 0.1, 0 ;rng.disttype = CV_RAND_NORMAL;cvRa nd( &rng, state ;memcpy( cv

50、kalman->transition_matrix->data.fl, A,sizeof (A;memcpy( cvkalman->measurement_matrix->data.fl, H,sizeof (H;memcpy( cvkalma n->process_ no ise_cov->data.fl, Q,sizeof (Q;memcpy( cvkalman->error_cov_post->data.fl, P,sizeof (P;memcpy( cvkalman->measurement_noise_cov->data.fl, R,sizeof (R;/cvSetIdentity( cvkalman->process_noise_cov, cvRealScalar(1e-5 ;/cvSetIdentity( cvkalman->error_cov_post, cvRealScalar(1;/cvSetIde ntity( cvkalma n-> measureme nt_no ise_cov, cvRealScalar(1e-1 ;/* choose in itial state */state->data.f