卡爾曼濾波MATLAB程式碼實現
沒有大量的公式推導，個人感覺也沒有必要，我們從小推導過很多公式，試著想想我們還能回憶起幾個？個人認為只需要記住公式的用法，作用，知道有這個公式就可以。用的時候我們可以隨時去查。所以樓主參考網上資料結合一個小例子整理出卡爾曼濾波的MATLAB程式碼實現。看懂這些程式碼我們需要對卡爾曼濾波演算法有基本的理解。大致的理解就是卡爾曼濾波演算法通過兩個量對系統進行最優估計，而這兩個量應該佔多大的權重，這就是卡爾曼濾波的核心所在。卡爾曼濾波通過不斷迭代（這也是為什麼卡爾曼濾波這麼多人研究的原因，計算機最會幹的事情就是迭代）計算出最佳的權重比。從而對系統進行最優估計。
這個例子就是被網上舉例舉爛了的房間溫度預測案列。
假設我們要研究的物件是一個房間的溫度。根據你的經驗判斷，這個房間的溫度是恆定的，也就是下一分鐘的溫度等於現在這一分鐘的溫度（假設我們用一分鐘來做時間單位）。假設你對你的經驗不是100%的相信，可能會有上下偏差幾度。我們把這些偏差看成是高斯白噪聲（White Gaussian Noise），也就是這些偏差跟前後時間是沒有關係的而且符合高斯分配（Gaussian Distribution）。另外，我們在房間裡放一個溫度計，但是這個溫度計也不準確的，測量值會比實際值偏差。我們也把這些偏差看成是高斯白噪聲。
好了，現在對於某一分鐘我們有兩個有關於該房間的溫度值：你根據經驗的預測值（系統的預測值）和溫度計的值（測量值）。下面我們要用這兩個值結合他們各自的噪聲來估算出房間的實際溫度值。
假如我們要估算k時刻的是實際溫度值。首先你要根據k-1時刻的溫度值，來預測k時刻的溫度。因為你相信溫度是恆定的，所以你會得到k時刻的溫度預測值是跟k-1時刻一樣的，假設是23度，同時該值的高斯噪聲的偏差是5度（5是這樣得到的：如果k-1時刻估算出的最優溫度值的偏差是3，你對自己預測的不確定度是4度，他們平方相加再開方，就是5）。然後，你從溫度計那裡得到了k時刻的溫度值，假設是25度，同時該值的偏差是4度。
由於我們用於估算k時刻的實際溫度有兩個溫度值，分別是23 度和25度。究竟實際溫度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點，我們可以用他們的covariance（協方差）來判斷。因為 Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我們可以估算出k時刻的實際溫度值是：23+0.78*(25-23)=24.56度。可以看出，因為溫度計的covariance比較小（比較相信溫度計），所以估算出的最優溫度值偏向溫度計的值。
現在我們已經得到k時刻的最優溫度值了，下一步就是要進入 k+1時刻，進行新的最優估算。到現在為止，好像還沒看到什麼自迴歸的東西出現。對了，在進入k+1時刻之前，我們還要算出k時刻那個最優值（24.56 度）的偏差。演算法如下：((1-Kg)*5^2)^0.5=2.35。這裡的5就是上面的k時刻你預測的那個23度溫度值的偏差，得出的2.35就是進入 k+1時刻以後k時刻估算出的最優溫度值的偏差（對應於上面的3）。
就是這樣，卡爾曼濾波器就不斷的把 covariance遞迴，從而估算出最優的溫度值。他執行的很快，而且它只保留了上一時刻的covariance。上面的Kg，就是卡爾曼增益（Kalman Gain）。
我們理解了這個例子之後也不需要進行公示推導，我們只需要記住卡爾曼濾波的五個關鍵步驟即可。
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))
下面對應的上邊那個房間溫度預測的MATLAB程式碼
x(k) = x(k - 1);%預估計k時刻狀態變數的值
p = p + Q;%對應於預估值的協方差
kg = p / (p + R);%卡爾曼增益
x(k) = x(k) + kg * (y(k) - x(k));
p = (1 - kg) * p;
clear
clc;
N=300;
CON = 25;%房間溫度，假定溫度是恆定的
%%%%%%%%%%%%%%%kalman filter%%%%%%%%%%%%%%%%%%%%%%
x = zeros(1,N);
y = 2^0.5 * randn(1,N) + CON;%加過程噪聲的狀態輸出
x(1) = 1;
p = 10;
Q = cov(randn(1,N));%過程噪聲協方差
R = cov(randn(1,N));%觀測噪聲協方差
for k = 2 : N
x(k) = x(k - 1);%預估計k時刻狀態變數的值
p = p + Q;%對應於預估值的協方差
kg = p / (p + R);%kalman gain
x(k) = x(k) + kg * (y(k) - x(k));
p = (1 - kg) * p;
end
%%%%%%%%%%這一部分是對卡曼濾波演算法的改進，通過加窗進行平滑處理%%%%%%%%%%%%%%%%%%%%%%%%
Filter_Wid = 10;
smooth_res = zeros(1,N);
for i = Filter_Wid + 1 : N
tempsum = 0;
for j = i - Filter_Wid : i - 1
tempsum = tempsum + y(j);
end
smooth_res(i) = tempsum / Filter_Wid;
end
% figure(1);
% hist(y);
t=1:N;
figure(1);
expValue = zeros(1,N);
for i = 1: N
expValue(i) = CON;
end
plot(t,expValue,'r',t,x,'g',t,y,'b',t,smooth_res,'k');
legend('expected','estimate','measure','smooth result');
axis([0 N 20 30])
xlabel('Sample time');
ylabel('Room Temperature');
title('Smooth filter VS kalman filter');