卡爾曼濾波與matlab模擬

09-16

卡爾曼濾波與matlab模擬

4 人贊了文章

卡爾曼濾波是一種遞歸的估計，即只要獲知上一時刻狀態的估計值以及當前狀態的觀測值就可以計算出當前狀態的估計值，因此不需要記錄觀測或者估計的歷史信息。

框圖如下：

這裡不涉及具體的推導過程，站在巨人的肩膀上，直接使用結果：

卡爾曼濾波器的遞歸過程：

1) 估計時刻k 的狀態：

X(k) = A*X(k-1) + B*u(k)

這裡, u(k) 是系統輸入

2) 計算誤差相關矩陣P, 度量估計值的精確程度：

P(k) = A*P(k-1)*A』+ Q

這裡, Q = E{ Wj^2 } 是系統雜訊的協方差陣，即系統框圖中的Wj的協方差陣, Q 應該是不斷變化的，為了簡化，當作一個常數矩陣。

3) 計算卡爾曼增益, 以下略去 (k), 即 P = P(k), X = X(k)：

K = P C』 (C * P * C』 + R) -1

這裡 R = E{ Vj^2 }, 是測量雜訊的協方差(陣), 即系統框圖中的 Vj 的協方差, 為了簡化，也當作一個常數矩陣。由於我們的系統一般是單輸入單輸出，所以 R是一個 1x1的矩陣，即一個常數，上面的公式可以簡化為:

K = P C』 / (C P * C』 + R)

4) 狀態變數反饋的誤差量：

e = Z(k) – C*X(k)

這裡的 Z(k) 是帶雜訊的測量量

5) 更新誤差相關矩陣P

P = P – K * C * P

6) 更新狀態變數：

X =X + K*e = X + K* (Z(k) – C*X(k))

7) 最後的輸出：

Y = C*X

matlab 模擬：

%卡爾曼濾波實例%測量房間溫度，房間溫度真實值為T=25度，一共測量兩百個點N=200; T=25; size=[N,1]; %取溫度預測值的方差為Q=1e-3,溫度感測器的測量方差為R=0.36，即我們更相信預測值，而較少相信感測器測量值。Q=1e-3; R=0.36; T_mearsured=T+sqrt(R)*randn(size);%初始時刻溫度的最優估計值為T_start=22.5度,溫度初始估計方差為P_start=2T_start=22.5; P_start=2;T_kalman(1)=T_start; P_kalman(1)=P_start;%用_kalman的後綴表示最優估計值，用_pre的後綴表示預測值for k=2:N%在進行溫度預測時，因為溫度是一個連續的狀態，我們認為上一時刻的溫度和當前時刻的溫度相等，則有T(k)=T(k-1)。T_pre(k)=T_kalman(k-1);P_pre(k)=P_kalman(k-1)+Q;K(k)=P_pre(k)/(P_pre(k)+R);T_kalman(k)=T_pre(k)+K(k)*(T_mearsured(k)-T_pre(k));P_kalman(k)=P_pre(k)-K(k)*P_pre(k);end%畫圖figure();plot(T*ones(size),g);hold onplot(T_mearsured,b);hold onplot(T_kalman,r);legend(溫度真實值,溫度測量值,Kalman估計值)