卡爾曼濾波器的原理及應(yīng)用

最近在學(xué)習(xí)Probablistic Robotics這本書，獲益良多,。以前學(xué)了概率論和隨機過程之后一直覺得這些是虛的,，不知道在工程上怎么用，而這本書恰恰就是講如何把這些概率理論和方差估計應(yīng)用到工程上去,，更確切的說,，應(yīng)用到機器人上去。

• 應(yīng)用前提
• 算法詳細(xì)介紹
• 應(yīng)用舉例
• 下一步

1.應(yīng)用前提

要應(yīng)用kalman Filter,首先要有三個前提假設(shè)：

• 當(dāng)前狀態(tài)的概率分布必須是上一狀態(tài)和將要執(zhí)行的控制量的線性函數(shù),，再疊加一個高斯噪聲,。表達(dá)式如下：
  
  其中和是狀態(tài)變量，如果系統(tǒng)有多個自由度的話,，它們表示狀態(tài)向量,。這樣的話根據(jù)高斯分布
  可以得到狀態(tài)轉(zhuǎn)移概率

  
  

  其中表示上一狀態(tài)的均值,，表示方差。

• 對狀態(tài)的測量必須是狀態(tài)的線性函數(shù)疊加高斯噪聲
  

  其含義與上類似,，不再贅述,。

• 初始狀態(tài)分布為高斯分布

  

2.算法詳細(xì)介紹

Kalman Filter五條黃金公式 ：

這五條公式基本上就是Kalman Filter的主要內(nèi)容了， 它的本質(zhì)就是通過預(yù)測結(jié)合測量來估計當(dāng)前系統(tǒng)的狀態(tài),。舉個lizi,，假如我們要估計一架飛行器的姿態(tài)，可以通過IMU來實時測量,，但是測量值有一定的風(fēng)險是不準(zhǔn)確的,，所以并不能完全依賴傳感器。任何一個滿足物理規(guī)律的系統(tǒng)應(yīng)當(dāng)是連續(xù)的,，所以我們還可以通過上一狀態(tài)來預(yù)測當(dāng)前狀態(tài),。Kalman Filter正是結(jié)合這兩條進行狀態(tài)估計，到底是相信哪一個多一點,，還要根據(jù)Kt來決定,，我們定義Kt為卡爾曼增益，它是根據(jù) 測量和預(yù)測的協(xié)方差來計算的,。

下面逐條解釋：

• line 2： 首先通過上一狀態(tài)最優(yōu)值和將要施加的控制量來預(yù)測當(dāng)前狀態(tài),，由假設(shè)一可以得到：

  

  因為我們只是求均值，而高斯噪聲均值為0,，所以可省去最后一項,。

• line 3： 除了預(yù)測均值之外，我們還需要預(yù)測值的協(xié)方差來計算Kalman增益,。

  

  根據(jù)假設(shè)2,，這條公式可以很容易得到。

• line 4：準(zhǔn)備工作完成之后,，需要根據(jù)預(yù)測值的協(xié)方差,，測量值和狀態(tài)

  的比例系數(shù)，測量值的協(xié)方差來計算Kalman增益,。

  

  具體證明需要用到假設(shè)中的高斯分布公式,，因為我們只是應(yīng)用，所以就不在blog中討論啦,，感興趣的小伙伴可以看書中3.2.4節(jié) Mathematical Derivation of the KF,，里面講的很詳細(xì)，分享一下下載鏈接
  http://download.csdn.net/detail/lizilpl/8632071,。

• line 5：這一行可以說是Kalman Filter 的精華了,，現(xiàn)在我們有了對狀態(tài)的預(yù)測值和協(xié)方差，同時也收集到了對狀態(tài)的測量值。這時就可以通過kalman增益來計算狀態(tài)估計值了,。

  

  增益越大,，表明我們越相信測量值。

• line 6： 根據(jù) line3 ,，預(yù)測當(dāng)前狀態(tài)需要用到上一狀態(tài)的協(xié)方差，所以我們還需要計算當(dāng)前狀態(tài)的協(xié)方差用于下一次迭代,。它同樣要根據(jù)Kalman增益來計算：

  

  相信到這里,，大家應(yīng)該對kalman Filter的原理有了一個大致的了解，算法中,，從初始狀態(tài)開始,，不斷計算當(dāng)前狀態(tài)的均值和方差來迭代，直至系統(tǒng)結(jié)束,。

  3.應(yīng)用實例

  如下程序引用自百度百科
  http://baike.baidu.com/link?url=g11J2Ab9SHiYaGB34hl86UxEMnaJyXwi_I5SrTrzKDYEMSynK1zO1Is0oXVZkR1yNKbtudaGws8j7NAdkLuV8q
  為方便閱讀,，把原代碼中關(guān)于作者版權(quán)的信息注釋掉了，如有侵權(quán),，請聯(lián)系我,，我會第一時間補上。

  程序描述了如下一個系統(tǒng)：

  • 房間溫度為24度
  • 房間內(nèi)連續(xù)兩個時刻溫度差值的標(biāo)準(zhǔn)差為0.02度
  • 溫度計的測量值誤差的標(biāo)準(zhǔn)差為0.5度
  • 對溫度的初始估計值為23.5度,，誤差的方差為1
  • 對整個系統(tǒng)的控制量為0

  現(xiàn)在需要利用Kalman Filter來估計房間的實時溫度

  % 初始化參數(shù)n_iter = 100; %計算連續(xù)n_iter個時刻sz = [n_iter, 1]; x = 24; % 溫度的真實值Q = 4e-4; % 對溫度預(yù)測值的方差R = 0.25; % 測量方差,，反應(yīng)溫度計的測量精度T_start = 23.5; %溫度初始估計值delta_start = 1; %溫度初始估計方差z = x + sqrt(R)*randn(sz); % z是溫度計的測量結(jié)果，在真實值的基礎(chǔ)上加上了方差為0.25的高斯噪聲,。% 初始化數(shù)組state_kalman=zeros(sz); % 對溫度的估計值,。即在k時刻，結(jié)合溫度計當(dāng)前測量值與k-1時刻預(yù)測值,，得到的最終估計值variance_kalman=zeros(sz); % 估計值的方差state_pre=zeros(sz); % 對溫度的預(yù)測variance_pre=zeros(sz); % 預(yù)測值的方差K=zeros(sz); % 卡爾曼增益state_kalman(1) = T_start; %溫度估計值初始化variance_kalman(1) =delta_start; %估計值方差初始化%%開始迭代計算for k = 2:n_iterstate_pre(k) = state_kalman(k-1);%用上一時刻的最優(yōu)估計值來作為對當(dāng)前時刻的溫度的預(yù)測variance_pre(k) = variance_kalman(k-1)+Q;%預(yù)測的方差為上一時刻溫度最優(yōu)估計值的方差與高斯噪聲方差之和%%計算卡爾曼增益K(k) = variance_pre(k)/( variance_pre(k)+R ); %%結(jié)合當(dāng)前時刻溫度計的測量值,，對上一時刻的預(yù)測進行校正，得到校正后的最優(yōu)估計,。由于是直接測量,，故C為1.state_kalman(k) = state_pre(k)+K(k)*(z(k)-state_pre(k)); variance_kalman(k) = (1-K(k))*variance_pre(k); %計算最終估計值的方差用于下一次迭代end%繪圖相關(guān)。,。,。。,。FontSize=14;LineWidth=3;figure();plot(z,'k+'); %畫出溫度計的測量值hold on;plot(state_kalman,'b-','LineWidth',LineWidth) %畫出最優(yōu)估計值hold on;plot(x*ones(sz),'g-','LineWidth',LineWidth); %畫出真實值legend('溫度測量值', 'Kalman估計值', '真實值');xl=xlabel('時間(分鐘)');yl=ylabel('溫度');set(xl,'fontsize',FontSize);set(yl,'fontsize',FontSize);hold off;set(gca,'FontSize',FontSize);figure();valid_iter = [2:n_iter]; % variance_pre not valid at step 1plot(valid_iter,variance_kalman([valid_iter]),'LineWidth',LineWidth); %畫出最優(yōu)估計值的方差legend('Kalman估計的誤差估計');xl=xlabel('時間(分鐘)');yl=ylabel('℃^2');set(xl,'fontsize',FontSize);set(yl,'fontsize',FontSize);set(gca,'FontSize',FontSize);

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60

  “`
  運行結(jié)果如下：
  
  

  由結(jié)果可以看出來,，雖然測量值噪聲很大，但Kalman Filter的預(yù)測值還是逐漸逼近了真實溫度,，并且估計值的方差逐漸收斂,！

  4.下一步

  如上討論，經(jīng)典的卡爾曼濾波只適用于線性且滿足高斯分布的系統(tǒng)，但實際工程中并不是這么簡單,，比如飛行器在水平運動時有可能伴隨著自身的自旋,，這時就需要應(yīng)用擴展卡爾曼濾波（EKF）來解決這種情況。等我研究完之后和大家分享,。