卡爾曼濾波算法及C語言實(shí)現(xiàn)

摘要：本文著重討論了卡爾曼濾波器的原理，典型算法以及應(yīng)用領(lǐng)域。清晰地闡述了kalman filter在信息估計(jì)方面的最優(yōu)性能。著重介紹簡(jiǎn)單kalman filter algorithm的編程，使用kalman filter的經(jīng)典5個(gè)體現(xiàn)最優(yōu)化遞歸公式來編程。通過c語言編寫程序?qū)崿F(xiàn)kalman filter的最優(yōu)估計(jì)能力。

1 引言

Kalman Filter是一個(gè)高效的遞歸濾波器，它可以實(shí)現(xiàn)從一系列的噪聲測(cè)量中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)。起源于Rudolf Emil Kalman在1960年的博士論文和發(fā)表的論文《A New Approach to Linear Eiltering and Prediction Problems》（《線性濾波與預(yù)測(cè)問題的新方法》）。并且最先在阿波羅登月計(jì)劃軌跡預(yù)測(cè)上應(yīng)用成功，此后kalman filter取得重大發(fā)展和完善。它的廣泛應(yīng)用已經(jīng)超過30年，包括機(jī)器人導(dǎo)航，控制。傳感器數(shù)據(jù)融合甚至在軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤等等，近年來更被廣泛應(yīng)用于計(jì)算機(jī)圖像處理，例如頭臉識(shí)別，圖像分割，圖像邊緣檢測(cè)等等。

2 kalman filter最優(yōu)化遞歸估計(jì)

Kalman filter是一個(gè)“optimal recursive data processing algorithm（最優(yōu)化遞歸數(shù)據(jù)處理方法）”。對(duì)于解決很大部分的問題，他是最優(yōu)，效率最高甚至是最有用的方法。而kalman filter最為核心的內(nèi)容是體現(xiàn)它最優(yōu)化估計(jì)和遞歸特點(diǎn)的5條公式。舉一個(gè)例子來詳細(xì)說明5條公式的物理意義。

假設(shè)我們要研究的對(duì)象是某一個(gè)房間的溫度信號(hào)。對(duì)于室溫來說，一分鐘內(nèi)或一小段時(shí)間內(nèi)的值是基本上不變的或者變化范圍很小。也就是說時(shí)刻的溫度和時(shí)刻的溫度基本不變，即。在這個(gè)過程中，因?yàn)楫吘箿囟冗�是有所改變的，設(shè)有幾度的偏差。我們把這幾度的偏差看成是高斯白噪聲，也就是說，。除此之外我們?cè)谟靡粋�(gè)溫度計(jì)來實(shí)時(shí)測(cè)量房間的溫度值，但由于量具本身的誤差，所測(cè)得的溫度值也是不準(zhǔn)確的，也會(huì)和實(shí)際值偏差幾度，把這幾度的偏差看成是測(cè)量噪聲。即滿足，。

此時(shí)我們對(duì)于這個(gè)房間的溫度就得到了兩個(gè)數(shù)據(jù)。一個(gè)是你根據(jù)經(jīng)驗(yàn)得到的經(jīng)驗(yàn)值，一個(gè)是從溫度計(jì)上得到的測(cè)量值，以及各自引入的高斯白噪聲。下面就具體講解kalman filter來估計(jì)房間溫度的原理與步驟。

要估計(jì)K時(shí)刻的實(shí)際溫度值，首先要根據(jù)K-1時(shí)刻的溫度值預(yù)測(cè)K時(shí)刻的溫度，按照之前我們討論的，若k-1時(shí)刻的溫度值是，那么預(yù)測(cè)此時(shí)的，假如該值的噪聲是，5°是這樣得到的，若果k-1時(shí)刻估算出的最優(yōu)溫度值的噪聲是，預(yù)測(cè)的噪聲是，所以總體的噪聲為。此時(shí)再從溫度計(jì)上得到K時(shí)刻的溫度值為，設(shè)該測(cè)量值的噪聲是。

現(xiàn)在發(fā)現(xiàn)問題了，在k時(shí)刻我們就有了兩個(gè)溫度值和，要信那個(gè)呢，簡(jiǎn)單的求平均已經(jīng)不能滿足精度的要求了。我們可以用他們的協(xié)方差covariance來判斷。協(xié)方差本身就能體現(xiàn)兩個(gè)信號(hào)的相關(guān)性，通過它就能判斷到底真值更逼近于預(yù)測(cè)值還是測(cè)量值。引入kalman gain（），有公式計(jì)算，

      ……（1）

所以=0.78。我們可以估算出K時(shí)刻的實(shí)際溫度值是，

   ……（2）

可以看出這個(gè)值接近于溫度計(jì)測(cè)量到的值，所以估算出的最優(yōu)溫度值偏向溫度計(jì)的值。

這時(shí)我們已經(jīng)得到了K時(shí)刻的最優(yōu)溫度值，接下來估計(jì)K+1時(shí)刻的最優(yōu)溫度值。既然kalman filter是一個(gè)最優(yōu)化的遞歸處理方法，那么遞歸就體現(xiàn)在該算法的一個(gè)核心參數(shù)上，由公式（1）的算法可知每次計(jì)算時(shí)的是不一樣的。這樣我們要估計(jì)K+1時(shí)刻的最優(yōu)溫度值，就得先算出K時(shí)刻的，然后才能利用公式（2）估計(jì)K+1時(shí)刻的最優(yōu)溫度值。由此可以看出我們只需知道初始時(shí)刻的值和它所對(duì)應(yīng)的協(xié)方差以及測(cè)量值，就可以進(jìn)行kalman估計(jì)了。

3Kalman Filter Algorithm

首先以一個(gè)離散控制過程為例討論kalman filter algorithm。該系統(tǒng)可用一個(gè)線性微分方程來描述。

……（3）

……（4）

（3）式和（4）式中，是K時(shí)刻的系統(tǒng)狀態(tài)，是K時(shí)刻對(duì)系統(tǒng)的控制量，A和B是系統(tǒng)參數(shù)，對(duì)于多模型系統(tǒng)，它們?yōu)榫仃嚒?img id="aimg_R3n4w" onclick="zoom(this, this.src, 0, 0, 0)" class="zoom" width="36" height="21" src="http://c.51hei.com/a/huq/a/a/c/23/23.038.jpg" border="0" alt="" />是K時(shí)刻的測(cè)量值，H是測(cè)量系統(tǒng)的參數(shù)，對(duì)于多測(cè)量系統(tǒng)，H為矩陣。和分別表示系統(tǒng)和測(cè)量過程中的噪聲，使用kalman filter估計(jì)時(shí)，我們認(rèn)為噪聲滿足高斯白噪聲模型，設(shè)和的covariance分別為Q和R。

討論kalman filter algorithm的5個(gè)經(jīng)典核心公式。

第一步，預(yù)測(cè)現(xiàn)在的狀態(tài)：

        ……（5）

式（5）中是利用上一狀態(tài)預(yù)測(cè)的結(jié)果，是上一時(shí)刻的最優(yōu)預(yù)測(cè)值，為現(xiàn)在狀態(tài)的控制量，如果沒有，可以為0。

經(jīng)過公式（5）后系統(tǒng)結(jié)果已經(jīng)更新了，對(duì)應(yīng)于 的covariance還沒有更新，用P表示covariance，

           ……（6）

式（6）中是對(duì)應(yīng)的covariance，是對(duì)應(yīng)的covariance，是的轉(zhuǎn)置矩陣。Q是系統(tǒng)的噪聲，（5）和（6）式便是kalman filter中的前兩個(gè)公式。對(duì)系統(tǒng)的預(yù)測(cè)。有了系統(tǒng)的預(yù)測(cè)，接下來就要參考測(cè)量值進(jìn)行估計(jì)了。

    ……（7）

由上面分析可知為了實(shí)現(xiàn)遞歸，每次的都是實(shí)時(shí)更新的。

       ……（8）

           ……（9）

這樣每次和都需要前一時(shí)刻的值來更新，遞歸的估計(jì)下去。（5）~（9）式便是kalman filter algorithm的五條核心公式。

4 利用C語言編程實(shí)現(xiàn)Kalman Filter Algorithm

要求是給定一個(gè)固定量，然后由測(cè)量值來使用kalman filter估計(jì)系統(tǒng)真實(shí)值。

為了編程簡(jiǎn)單，我將（5）式中的A=1，=0，（5）式改寫為下面的形式，

               ……（10）

式（6）改寫為，

               ……（11）

再令H=1，式（7），（8），（9）可改寫為，

       ……（12）

          ……（13）

             ……（14）

使用C語言編程實(shí)現(xiàn)（核心算法）。

                            x_mid=x_last;    //x_last=x(k-1|k-1),x_mid=x(k|k-1)

                            p_mid=p_last+Q;  //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪聲

                            kg=p_mid/(p_mid+R); //kg為kalman filter，R為噪聲

                            z_measure=z_real+frand()*0.03;//測(cè)量值

                            x_now=x_mid+kg*(z_measure-x_mid);//估計(jì)出的最優(yōu)值

                            p_now=(1-kg)*p_mid;//最優(yōu)值對(duì)應(yīng)的covariance

        p_last = p_now;  //更新covariance值

        x_last = x_now;  //更新系統(tǒng)狀態(tài)值

5 算法測(cè)試

為了測(cè)試kalman filter algorithm，我設(shè)計(jì)了一個(gè)簡(jiǎn)單實(shí)驗(yàn)，來驗(yàn)證kalman filter的優(yōu)良性。程序中給定一個(gè)初值，然后給定一組測(cè)量值，驗(yàn)證kalman filter估值的準(zhǔn)確性。

根據(jù)kalman filter algorithm，我們需要給定系統(tǒng)初始值x_last，系統(tǒng)噪聲Q和測(cè)量噪聲R，以及初始值所對(duì)應(yīng)的協(xié)方差P_last。為了驗(yàn)證優(yōu)劣性，還需要給定真實(shí)值z(mì)_real來計(jì)算kalman filter誤差error_kalman以及測(cè)量誤差error_measure以及它們?cè)谟邢薮蔚挠?jì)算中的累積誤差，累積kalman誤差sumerror_kalman和累積測(cè)量誤差sumerror_measure。

實(shí)驗(yàn)中給定x_last=0，p_last=0,Q=0.018,R=0.0542.實(shí)驗(yàn)中可以通過適當(dāng)改變Q和R來獲得更好的估計(jì)結(jié)果。也可以改變p_last和x_last的值，由于kalman filter是對(duì)協(xié)方差的遞歸算法來估計(jì)信號(hào)數(shù)據(jù)的，所以p_last對(duì)算法結(jié)果的影響很大，圖3就說明了這一情況，由于在初始時(shí)就有協(xié)方差，所以在運(yùn)行過程中算法累積誤差相比初始時(shí)沒有誤差的就比較大。

給定值為z_real=0.56時(shí)運(yùn)行結(jié)果如圖1所示：

給定值z(mì)_real=3.32時(shí)的運(yùn)行結(jié)果如圖2

圖3為Q，R不變，p_last=0.02，x_last=0，z_real=0.56時(shí)的測(cè)試結(jié)果。通過和前兩次結(jié)果比較發(fā)現(xiàn)p_last對(duì)估計(jì)結(jié)果影響較大，分析可知這種現(xiàn)象是符合kalman filter的，通過改變Q和R的值也能改善算法的性能，但是實(shí)際操作中我們并不能控制這兩個(gè)量。

6 結(jié)論

本文通過對(duì)kalman filter algorithm的深入探討，對(duì)kalman filter有了更深刻的認(rèn)識(shí)，理解了核心的5條公式的物理意義，以及kalman filter的思想，并通過理解算法編程實(shí)踐，驗(yàn)證了kalman filter在數(shù)據(jù)處理方面的優(yōu)良性能。

并且通過實(shí)驗(yàn)結(jié)果分析了kalman filter algorithm的本質(zhì)對(duì)每次估計(jì)產(chǎn)生的協(xié)方差遞歸結(jié)合當(dāng)前測(cè)量值來估計(jì)系統(tǒng)當(dāng)前的最佳狀態(tài)。如要改善算法的性能就必須要盡可能的減小系統(tǒng)噪聲和測(cè)量噪聲，優(yōu)化程序，減小估計(jì)的協(xié)方差。

參考文獻(xiàn)

[1]譚浩強(qiáng).C程序設(shè)計(jì)(第三版)[M].北京:清華大學(xué)出版社,2005,91~130.

[2]崔平遠(yuǎn),黃曉瑞.基于聯(lián)合卡爾曼濾波的多傳感器信息融合算法及其應(yīng)用[J].電機(jī)與控制學(xué)報(bào),2001,9(5): 204-207.

[3]黨宏社,韓崇昭,段戰(zhàn)勝.基于多卡爾曼濾波器的自適應(yīng)傳感器融合[J].系統(tǒng)工程與電子技術(shù),2004,5(26):311-313.

[4]文貢堅(jiān),王潤生. 一種穩(wěn)健的直線提取算法[J].軟件學(xué)報(bào),2001,11(11):1660-1665.

}

以上圖文的Word格式文檔下載(內(nèi)容和本網(wǎng)頁上的一模一樣,方便保存):
卡爾曼濾波算法C語言實(shí)現(xiàn).zip (99.89 KB, 下載次數(shù): 445)

2018-12-14 11:09 上傳

點(diǎn)擊文件名下載附件
下載積分: 黑幣 -5