卡爾曼濾波模型及其Matlab實現

標籤：控制理論   預測   matlab   遞迴   濾波   

卡爾曼濾波建立在隱馬爾科夫模型上，是一種遞迴估計。也就是說，只需要知道上一個狀態的估計值，以及目前狀態的觀測值，就能計算目前狀態的最優估計值。

而不需要更早的曆史資訊。

 

卡爾曼濾波器的2個狀態

1.最優估計

2.誤差共變數矩陣

這兩個變數迭代計算，初始值多少，其實沒有影響。反正最後都能收斂到最優估計。

 

預測過程

F是狀態轉移矩陣，B是控制矩陣（也可以不需要）。Q是過程雜訊的共變數。

這裡等式左邊小勾勾表示估計量，有個負號表示，這個估計量還不優，差點東西。

 

更新過程

第1個式子，是狀態更新過程。H是測量矩陣，Z是觀察矩陣，括弧裡是測量殘差。

第2個式子，是卡爾曼增益矩陣。R是觀察雜訊的共變數矩陣。

第3個式子，是誤差共變數矩陣更新過程。

 

於是可以開始迭代了。我們以小汽車的[位置速度]為狀態變數，小車做勻速運動。

Z=(1:100)+0.1*randn(1,100);%觀測值加方差為1的白色雜訊X=[0.8;1.2];%初始最優估計狀態P=[1.2 0.9;0.8 1.3];%初始最優共變數矩陣F=[1 1;0 1];%狀態轉移矩陣Q=[0.001 0;0 0.001];%預測雜訊共變數矩陣H=[1 0];%觀測矩陣R=1;%觀測雜訊共變數矩陣hold onfor i=1:100    X_=F*X;%這裡沒有控制量    P_=F*P*F'+Q;    K=P_*H'/(H*P_*H'+R);    X=X_+K*(Z(i)-H*X_);    P=(eye(2)-K*H)*P_;    plot(X(1),X(2),'*');    end

可以看到，雖然初始狀態隨便寫，但是很快就收斂到了真實值附近。預測雜訊共變數矩陣Q要小一點才行，這表示我們對狀態轉移矩陣的信心足夠大。否則預測效果會很差。

歡迎參與討論並關注本部落格和微博以及知乎個人首頁後續內容繼續更新哦~

轉載請您尊重作者的勞動，完整保留上述文字以及文章連結，謝謝您的支援！