卡爾曼濾波器的原理及應用

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

• 應用前提
• 算法詳細介紹
• 應用舉例
• 下一步

---

1.應用前提

要應用kalman Filter,首先要有三個前提假設：

• 當前狀態的概率分布必須是上一狀態和將要執行的控制量的線性函數，再疊加一個高斯噪聲。表達式如下：?
  ?
  其中和是狀態變量，如果系統有多個自由度的話，它們表示狀態向量。這樣的話根據高斯分布?
  可以得到狀態轉移概率

  ?
  

  其中表示上一狀態的均值，表示方差。

• 對狀態的測量必須是狀態的線性函數疊加高斯噪聲?
  

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

• 初始狀態分布為高斯分布

2.算法詳細介紹

Kalman Filter五條黃金公式?：

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

下面逐條解釋：

• line 2：?首先通過上一狀態最優值和將要施加的控制量來預測當前狀態，由假設一可以得到：

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

• line 3：?除了預測均值之外，我們還需要預測值的協方差來計算Kalman增益。

  根據假設2，這條公式可以很容易得到。

• line 4：準備工作完成之后，需要根據預測值的協方差，測量值和狀態

  的比例系數，測量值的協方差來計算Kalman增益。

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

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

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

• line 6：?根據?line3?，預測當前狀態需要用到上一狀態的協方差，所以我們還需要計算當前狀態的協方差用于下一次迭代。它同樣要根據Kalman增益來計算：

  相信到這里，大家應該對kalman Filter的原理有了一個大致的了解，算法中，從初始狀態開始，不斷計算當前狀態的均值和方差來迭代，直至系統結束。

  3.應用實例

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

  程序描述了如下一個系統：

  • 房間溫度為24度
  • 房間內連續兩個時刻溫度差值的標準差為0.02度
  • 溫度計的測量值誤差的標準差為0.5度
  • 對溫度的初始估計值為23.5度，誤差的方差為1
  • 對整個系統的控制量為0

  現在需要利用Kalman Filter來估計房間的實時溫度

  % 初始化參數 n_iter = 100; %計算連續n_iter個時刻 sz = [n_iter, 1]; x = 24; % 溫度的真實值 Q = 4e-4; % 對溫度預測值的方差 R = 0.25; % 測量方差，反應溫度計的測量精度 T_start = 23.5; %溫度初始估計值 delta_start = 1; %溫度初始估計方差 z = x + sqrt(R)*randn(sz); % z是溫度計的測量結果，在真實值的基礎上加上了方差為0.25的高斯噪聲。 % 初始化數組 state_kalman=zeros(sz); % 對溫度的估計值。即在k時刻，結合溫度計當前測量值與k-1時刻預測值，得到的最終估計值 variance_kalman=zeros(sz); % 估計值的方差 state_pre=zeros(sz); % 對溫度的預測 variance_pre=zeros(sz); % 預測值的方差 K=zeros(sz); % 卡爾曼增益 state_kalman(1) = T_start; %溫度估計值初始化 variance_kalman(1) =delta_start; %估計值方差初始化 % %開始迭代計算 for k = 2:n_iter state_pre(k) = state_kalman(k-1); %用上一時刻的最優估計值來作為對當前時刻的溫度的預測 variance_pre(k) = variance_kalman(k-1)+Q; %預測的方差為上一時刻溫度最優估計值的方差與高斯噪聲方差之和 % %計算卡爾曼增益 K(k) = variance_pre(k)/( variance_pre(k)+R ); % %結合當前時刻溫度計的測量值，對上一時刻的預測進行校正，得到校正后的最優估計。由于是直接測量，故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 %繪圖相關。。。。。 FontSize=14; LineWidth=3; figure(); plot(z,'k+'); %畫出溫度計的測量值 hold on; plot(state_kalman,'b-','LineWidth',LineWidth) %畫出最優估計值 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 1 plot(valid_iter,variance_kalman([valid_iter]),'LineWidth',LineWidth); %畫出最優估計值的方差 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

  “`?
  運行結果如下：?
  ?
  

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

  4.下一步

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

總結

以上是生活随笔為你收集整理的卡尔曼滤波器学习笔记（一）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。