卡爾曼濾波算法及C語言實現

摘要：本文著重討論了卡爾曼濾波器的原理，典型算法以及應用領域。清晰地闡述了kalman filter在信息估計方面的最優性能。著重介紹簡單kalman filter algorithm的編程，使用kalman filter的經典5個體現最優化遞歸公式來編程。通過c語言編寫程序實現kalman filter的最優估計能力。

1 引言

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

2 kalman filter最優化遞歸估計

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

假設我們要研究的對象是某一個房間的溫度信號。對于室溫來說，一分鐘內或一小段時間內的值是基本上不變的或者變化范圍很小。也就是說時刻的溫度和時刻的溫度基本不變，即。在這個過程中，因為畢竟溫度還是有所改變的，設有幾度的偏差。我們把這幾度的偏差看成是高斯白噪聲，也就是說，。除此之外我們在用一個溫度計來實時測量房間的溫度值，但由于量具本身的誤差，所測得的溫度值也是不準確的，也會和實際值偏差幾度，把這幾度的偏差看成是測量噪聲。即滿足，。

此時我們對于這個房間的溫度就得到了兩個數據。一個是你根據經驗得到的經驗值，一個是從溫度計上得到的測量值，以及各自引入的高斯白噪聲。下面就具體講解kalman filter來估計房間溫度的原理與步驟。

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

現在發現問題了，在k時刻我們就有了兩個溫度值和，要信那個呢，簡單的求平均已經不能滿足精度的要求了。我們可以用他們的協方差covariance來判斷。協方差本身就能體現兩個信號的相關性，通過它就能判斷到底真值更逼近于預測值還是測量值。引入kalman gain（），有公式計算，

      ……（1）

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

   ……（2）

可以看出這個值接近于溫度計測量到的值，所以估算出的最優溫度值偏向溫度計的值。

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

3Kalman Filter Algorithm

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

……（3）

……（4）

（3）式和（4）式中，是K時刻的系統狀態，是K時刻對系統的控制量，A和B是系統參數，對于多模型系統，它們為矩陣。是K時刻的測量值，H是測量系統的參數，對于多測量系統，H為矩陣。和分別表示系統和測量過程中的噪聲，使用kalman filter估計時，我們認為噪聲滿足高斯白噪聲模型，設和的covariance分別為Q和R。

討論kalman filter algorithm的5個經典核心公式。

第一步，預測現在的狀態：

        ……（5）

式（5）中是利用上一狀態預測的結果，是上一時刻的最優預測值，為現在狀態的控制量，如果沒有，可以為0。

經過公式（5）后系統結果已經更新了，對應于 的covariance還沒有更新，用P表示covariance，

           ……（6）

式（6）中是對應的covariance，是對應的covariance，是的轉置矩陣。Q是系統的噪聲，（5）和（6）式便是kalman filter中的前兩個公式。對系統的預測。有了系統的預測，接下來就要參考測量值進行估計了。

    ……（7）

由上面分析可知為了實現遞歸，每次的都是實時更新的。

       ……（8）

           ……（9）

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

4 利用C語言編程實現Kalman Filter Algorithm

要求是給定一個固定量，然后由測量值來使用kalman filter估計系統真實值。

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

               ……（10）

式（6）改寫為，

               ……（11）

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

       ……（12）

          ……（13）

             ……（14）

使用C語言編程實現（核心算法）。

                            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;//測量值

                            x_now=x_mid+kg*(z_measure-x_mid);//估計出的最優值

                            p_now=(1-kg)*p_mid;//最優值對應的covariance

        p_last = p_now;  //更新covariance值

        x_last = x_now;  //更新系統狀態值

5 算法測試

為了測試kalman filter algorithm，我設計了一個簡單實驗，來驗證kalman filter的優良性。程序中給定一個初值，然后給定一組測量值，驗證kalman filter估值的準確性。

根據kalman filter algorithm，我們需要給定系統初始值x_last，系統噪聲Q和測量噪聲R，以及初始值所對應的協方差P_last。為了驗證優劣性，還需要給定真實值z_real來計算kalman filter誤差error_kalman以及測量誤差error_measure以及它們在有限次的計算中的累積誤差，累積kalman誤差sumerror_kalman和累積測量誤差sumerror_measure。

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

給定值為z_real=0.56時運行結果如圖1所示：

給定值z_real=3.32時的運行結果如圖2

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

6 結論

本文通過對kalman filter algorithm的深入探討，對kalman filter有了更深刻的認識，理解了核心的5條公式的物理意義，以及kalman filter的思想，并通過理解算法編程實踐，驗證了kalman filter在數據處理方面的優良性能。

并且通過實驗結果分析了kalman filter algorithm的本質對每次估計產生的協方差遞歸結合當前測量值來估計系統當前的最佳狀態。如要改善算法的性能就必須要盡可能的減小系統噪聲和測量噪聲，優化程序，減小估計的協方差。

參考文獻

[1]譚浩強.C程序設計(第三版)[M].北京:清華大學出版社,2005,91~130.

[2]崔平遠,黃曉瑞.基于聯合卡爾曼濾波的多傳感器信息融合算法及其應用[J].電機與控制學報,2001,9(5): 204-207.

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

[4]文貢堅,王潤生. 一種穩健的直線提取算法[J].軟件學報,2001,11(11):1660-1665.

}

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

2018-12-14 11:09 上傳

點擊文件名下載附件
下載積分: 黑幣 -5