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

1、卡爾曼濾波算法及 C 語言實(shí)現(xiàn)摘要：本文著重討論了卡爾曼濾波器的原理，典型算法以及應(yīng)用領(lǐng)域。清晰地闡述了kalman filter 在信息估計(jì)方面的最優(yōu)性能。著重介紹簡(jiǎn)單 kalman filter algorithm 的編程，使用 kalman filter 的經(jīng)典 5filter 的最優(yōu)估計(jì)能力?，F(xiàn)最優(yōu)化遞歸公式來編程。通過 c 語言編寫程序?qū)崿F(xiàn) kalman：kalman filter；最優(yōu)估計(jì)；C 語言1 引言Kalman Filter 是一個(gè)高效的遞歸濾波器，它可以實(shí)現(xiàn)從一系列的噪聲測(cè)量中，估計(jì)動(dòng)態(tài)于 Rudolf Emil Kalman 在 1960 年的博士和的A New問題的新

2、方法）。并系統(tǒng)的狀態(tài)。Approach to Linear Eiltering and Prediction Problems（線性濾波與上應(yīng)用，此后 kalman filter 取得且最先在阿登月計(jì)劃軌跡發(fā)展和完善。它的廣泛應(yīng)用已經(jīng)超過 30 年，包括人導(dǎo)航，。傳感器數(shù)據(jù)融合甚至在軍事方面的雷達(dá)系統(tǒng)以及追蹤等等，近年來更被廣泛應(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ì)于解決

3、很大部分的問題，他是最優(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)的值是基本上不變的或者變化范圍很小。也就是說t1 時(shí)刻的溫度T1 和t2 時(shí)刻的溫度T2基本不變，即T2 = T1 。在這個(gè)過程中，因?yàn)楫吘箿囟冗€是有所改變的，設(shè)有幾度的偏差。我們把這幾度的偏差看成是高斯白噪聲 w(t) ，也就是說 Ew(t) = 0 ，Dw(t) = s 2 。除此之外我們?cè)谟靡粋€(gè)溫度計(jì)來實(shí)時(shí)測(cè)量房間的溫度值 Z

4、 ，但由于量具本身的誤差，所測(cè)得的溫度值也是確的，也會(huì)和實(shí)際值偏差幾度，把這幾度的偏差看成是測(cè)量噪聲v(t) 。即滿足 Ev(t) = 0 ， Dv(t) = s 。21此時(shí)我們對(duì)于這個(gè)房間的溫度就得到了兩個(gè)數(shù)據(jù)。一個(gè)是你根據(jù)經(jīng)驗(yàn)得到的經(jīng)驗(yàn)值1T2 = T1 ，一個(gè)是從溫度計(jì)上得到的測(cè)量值 Z ，以及各自引入的高斯白噪聲。下面就具體講解 kalman filter 來估計(jì)房間溫度的原理與步驟。要估計(jì) K 時(shí)刻的實(shí)際溫度值，首先要根據(jù) K-1 時(shí)刻的溫度值K 時(shí)刻的溫度，按照之前我們討論的 T = T，若 k-1 時(shí)刻的溫度值是 T= 230C ，那么 此時(shí)的 k -121T = T= 230C

5、 ，假如該值的噪聲是 w(k) =50C ，5°是這樣得到的，若果 k-1 時(shí)刻估算kk -1出的最優(yōu)溫度值的噪聲是 w(k)' =30C ，的噪聲是 v(k)' =40C ，所以總體的噪聲為w(k) =w(k)'2 + v(k)'2 =50C 。此時(shí)再從溫度計(jì)上得到 K 時(shí)刻的溫度值為T = 25oC ，設(shè)kz該測(cè)量值的噪聲是40C 。現(xiàn)在發(fā)現(xiàn)問題了，在 k 時(shí)刻我們就有了兩個(gè)溫度值T = 230C 和T = 25oC ，要信那個(gè)kkz呢，簡(jiǎn)單的求平均已經(jīng)不能滿足精度的要求了。我們可以用他們的協(xié)方差 covariance 來。協(xié)方差本身就能體現(xiàn)兩個(gè)信

6、號(hào)的相關(guān)性，通過它就能到底真值更逼近于值還是測(cè)量值。引入 kalman gain（ kg ），有公式計(jì)算kg ，222'2222kg = w(k) /(w(k) + v(k) ) = 5 /(5 + 4 )所以kg =0.78。我們可以估算出 K 時(shí)刻的實(shí)際溫度值是，（1）T = T + k ´(T -T ) = 23 + 0.78´ (25 - 23) = 24.56oC（2）kgkzk可以看出這個(gè)值接近于溫度計(jì)測(cè)量到的值，所以估算出的最優(yōu)溫度值偏向溫度計(jì)的值。這時(shí)我們已經(jīng)得到了 K 時(shí)刻的最優(yōu)溫度值，接下來估計(jì) K+1 時(shí)刻的最優(yōu)溫度值。既然參數(shù)kgkalman

7、 filter 是一個(gè)最優(yōu)化的遞歸處理方法，那么遞歸就體現(xiàn)在該算法的一個(gè)上，由公式（1） kg 的算法可知每次計(jì)算時(shí)的kg 是不一樣的。這樣我們要估計(jì) K+1 時(shí)刻的最優(yōu)溫度值，就得先算出 K 時(shí)刻的kg ，然后才能利用公式（2）估計(jì) K+1 時(shí)刻的最優(yōu)溫度值。由此可以看出我們只需知道初始時(shí)刻的值和它所對(duì)應(yīng)的協(xié)方差以及測(cè)量值，就可以進(jìn)行kalman 估計(jì)了。3 Kalman Filter Algorithm首先以一個(gè)離散程來描述。過程為例討論 kalman filter algorithm。該系統(tǒng)可用一個(gè)線性微分方X (k) = A× X (k -1) + B ×U (k)

8、 +W (k) （3）2Z(k) = H × X (k) +V (k) （4）（3）式和（4）式中， X (k) 是 K 時(shí)刻的系統(tǒng)狀態(tài)，U (k) 是 K 時(shí)刻對(duì)系統(tǒng)的量，A 和 B 是系統(tǒng)參數(shù)，對(duì)于多模型系統(tǒng)，它們?yōu)榫仃嚒?Z (k) 是 K 時(shí)刻的測(cè)量值，H 是測(cè)量系統(tǒng)的參數(shù)，對(duì)于多測(cè)量系統(tǒng)，H 為矩陣。W (k) 和V (k) 分別表示系統(tǒng)和測(cè)量過程中的噪聲，使用kalman filter 估計(jì)時(shí)，我們認(rèn)為噪聲滿足高斯白噪聲模型，設(shè)W (k) 和V (k) 的 covariance分別為 Q 和R。討論kalman filter algorithm 的 5 個(gè)經(jīng)典公式。第一步

9、，現(xiàn)在的狀態(tài)：X (k | k -1) = A× X (k -1| k -1) + B ×U (k)（5）式（5）中 X (k | k -1) 是利用上一狀態(tài)的結(jié)果， X (k -1| k -1) 是上一時(shí)刻的最優(yōu)值，U (k) 為現(xiàn)在狀態(tài)的量，如果沒有，可以為 0。經(jīng)過公式（5）后系統(tǒng)結(jié)果已經(jīng)更新了，對(duì)應(yīng)于 X (k | k -1) 的 covariance 還沒有更新，用 P 表示 covariance，P(k | k -1) = A× P(k -1| k -1) AT + Q（6）式（6）中 P(k | k -1) 是 X (k | k -1) 對(duì)應(yīng)的 co

10、variance，P(k -1| k -1) 是 X (k -1| k -1)對(duì)應(yīng)的 covariance， AT 是 A 的轉(zhuǎn)置矩陣。Q 是系統(tǒng)的噪聲，（5）和（6）式便是 kalman filter中的前兩個(gè)公式。對(duì)系統(tǒng)的。有了系統(tǒng)的，接下來就要參考測(cè)量值進(jìn)行估計(jì)了。X (k | k) = X (k | k -1) + kg (k) ×(Z(k) - H × X (k | k -1)（7）由上面分析可知為了實(shí)現(xiàn)遞歸，每次的kg 都是實(shí)時(shí)更新的。kg (k) = P(k | K -1) × H /(H × P(k | k -1) × H +

11、R)TT（8）P(k | k) = (1- kg (k) × H ) × P(k | k -1)（9）這樣每次 P(k | k) 和 kg (k) 都需要前一時(shí)刻的值來更新，遞歸的估計(jì)下去。（5）（9）式便是 kalman filter algorithm 的五條公式。4 利用 C 語言編程實(shí)現(xiàn) Kalman Filter Algorithm3要求是給定一個(gè)固定量，然后由測(cè)量值來使用 kalman filter 估計(jì)系統(tǒng)真實(shí)值。為了編程簡(jiǎn)單，我將（5）式中的 A=1，U (k) =0，（5）式改寫為下面的形式，X (k | k -1) = X (k -1| k -1)（10

12、）式（6）改寫為，P(k | k -1) = P(k -1| k -1) + Q（11）再令 H=1，式（7），（8），（9）可改寫為，X (k | k) = X (k | k -1) + kg (k) ×(Z(k) - X (k | k -1)（12）kg (k) = P(k | K -1) /(P(k | k -1) + R)（13）P(k | k) = (1- kg (k) × P(k | k -1)算法）。（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_mi

13、d=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)的 covariancep_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

14、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_real 來計(jì)算kalman filter 誤差error_kalman 以及測(cè)量誤差error_measure 以及它們?cè)谟邢薮蔚挠?jì)算中的累積誤差，累積kalman 誤差 sumerror_kalman 和累積測(cè)量誤差sumerror_measure。實(shí)驗(yàn)中給定 x_last=0，p_las

15、t=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 所示：4圖 1 真值為 0.56 的運(yùn)行結(jié)果給定值 z_real=3.32 時(shí)的運(yùn)行結(jié)果如圖 2圖 2 真值為 3.32 的運(yùn)行結(jié)果圖 3 為 Q，R 不變，p_last=0.

16、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è)量。5圖 3 改變 p_last 的測(cè)試結(jié)果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é)果分析了kal

17、man 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.北京:,2005,91130.2崔平遠(yuǎ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-166

18、5.6附錄：源程序#include "stdio.h" #include "stdlib.h" #include "math.h"double frand()return 2*(rand()/(double)RAND_MAX) - 0.5);/隨機(jī)噪聲void main()float float float float float float float float float float float float floatint i;x_last=0; p_last=0.02; Q=0.018; R=0.542;kg; x_mid;

19、 x_now; p_mid; p_now;z_real=0.56;/0.56 z_measure; sumerror_kalman=0;sumerror_measure=0;x_last=z_real+frand()*0.03; x_mid=x_last; for(i=0;i<20;i+) 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)的 covariancepri