在我總結Kalman filtering之前請允許我發泄一下，網上的各版本的卡爾曼濾波方程的變量字母真是多，而范例卻全都是同一個測量氣溫的簡單例子，單純看書的話公式自己又推不出來，真是日了狗了。
好了，說到卡爾曼濾波，我對卡爾曼濾波的初步理解就是（反正這句話也是抄的，看看就好了，我其實也不懂）：根據當前時刻的觀測值、上一時刻的預測值及預測誤差,計算得到當前的最優量去預測下一刻的量。至於對卡爾曼濾波意義理解的話可以在知乎上搜一下，有個測豬體重的例子感覺十分生動。公式推導的話智商過低的本人也是推不出來的，所以在此僅希望幫助大家學會運用，如果幫得上的話。
首先，我們引入一個離散控制過程的系統。該系統可用一個線性隨機微分方程來描述：
X(k)=AX(k-1)+BU(k)+W(k)
Z(k)=HX(k)+V(k)
X(k)是k時刻的系統狀態，U(k)是k時刻對系統的控制量。A和B是系統參數，對於多模型系統，他們為矩陣。Z(k)是k時刻的測量值，H是測量系統的參數，對於多測量系統，H為矩陣。W(k)和V(k)分別表示過程和測量的噪聲。他們被假設成高斯白噪聲，他們的協方差分別是Q，R。由於系統中一般不太有控制量，所以B這個參數一般為0，也就是沒有U(K)。
以下是編程需要的五個卡爾曼濾波的迭代方程：
首先利用系統的過程模型來預測系統下一狀態，設在k時刻的系統狀態為x(k),則可以根據系統模型，由上一狀態預測出現在狀態：
X(k|k-1)=AX(k-1|k-1)+Bu(k).....................(1)
其中x(k|k-1)是上一時刻的狀態對現在時刻狀態的預測，x(k-1|k-1)是上一時刻狀態的最優結果， u(k)為現在時刻狀態的控制量。
（主要看一下X(k|k-1)這樣的變量到底代表什么）
系統的狀態已經更新，現在需要更新系統的誤差估計協方差矩陣，用p(k|k-1)表示誤差估計協方差矩陣：
P(k|k-1)=A*P(k-1|k-1)A'+Q.......................(2)
（這個協方差的由來是由(1)式的預測方程得到的）
其中p(k|k-1)是在k時刻由上一狀態對此狀態的預測， p(k-1|k-1)是x(k-1|k-1)對應的誤差估計協方差矩陣，Q表示系統過程噪聲的協方差。
現在我們得到了預測結果，然后我們根據得到的現在狀態的測量值進行修正得到最優的估計量x(k|k):
X(k|k)=X(k|k-1)+Kg(k)*(Z(k)-Hx(k|k-1))....(3)
（常常在編程的時候會直接把（1）式直接代入這個式子，所以你們看不到（1）式）
(3)式中Kg(k)未知，則需要對其就行求解，就引出(4)式：
Kg(k)=P(k|k-1)*H'/(H*P(k|k-1)*H' + R)......(4)
(這個是卡爾曼增益，是用來修正預測值的一個參數)
到現在，我們以及得出的k 時刻的系統狀態的最優值x(k|k),為了讓卡爾曼濾波器不斷地進行下去，我們需要更新 x(k|k) 對應的p(k|k)
p(k|k)=(I-Kg(k)*H)*P(k|k-1).....................(5)
((3)(4)式計算出來的X(k|k)、P(k|k)又會迭代回(1)（2）式中，注意I是單位矩陣)
我們還是先來看看網上大肆流傳的一個例子吧。白字是原文，紅字是我的吐槽。
假設我們要研究的對象是一個房間的溫度。根據你的經驗判斷，這個房間的溫度是恆定的。（這里的假設相當於狀態方程的系數A為1）假設你對你的經驗不是100%的相信，可能會有上下偏差幾度，我們把這些偏差看成是高斯白噪聲（這里就是W(k)）。另外，我們在房間里放一個溫度計，但是這個溫度計也不准確的，測量值會比實際值偏差。我們也把這些偏差看成是高斯白噪聲。（溫度計的測量值就是Z(k),而由於溫度測到的溫度就是溫度，不用再換算，所以系數H就是1，偏差就是V(k)）。好了，現在對於某一分鍾我們有兩個有關於該房間的溫度值：你根據經驗的預測值（系統的預測值X(k|k-1)）和溫度計的值（測量值Z(k)）。下面我們要用這兩個值結合他們各自的噪聲來估算出房間的實際溫度值。
假如我們要估算k時刻的是實際溫度值。首先你要根據k-1時刻的溫度值，來預測k時刻的溫度。因為你相信溫度是恆定的，所以你會得到k時刻的溫度預測值是跟k-1時刻一樣的，假設是23度，同時該值的高斯噪聲的偏差是5度（5是這樣得到的：如果k-1時刻估算出的最優溫度值的偏差（p(k-1|k-1)就是上一時刻的p(k|k)，反正你懂）是3，你對自己預測的不確定度是4度（這個就是Q了，返回去看看黃色標注的Q的含義），他們平方相加再開方，就是5（算出來的就是P(k|k-1)））。然后，你從溫度計那里得到了k時刻的溫度值（測量值Z(k)），假設是25度，同時該值的偏差是4度（就是R）。由於我們用於估算k時刻的實際溫度有兩個溫度值，分別是23 度和25度。究竟實際溫度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點，我們可以用他們的covariance（協方差）來判斷。因為 Kg^2=5^2/(5^2+4^2)(該式的計算相當於上面的（4）式子)，所以Kg=0.78，我們可以估算出k時刻的實際溫度值是：23+0.78*(25-23)=24.56度（該式對應（3）式，就是算出最優估算值）。可以看出，因為溫度計的covariance比較小（比較相信溫度計），所以估算出的最優溫度值偏向溫度計的值。（我一直搞不懂這個協方差為什么一定要求幾何的誤差也就是為什么要平方再開根號，如果有人明白麻煩在評論里告訴我一下）
現在我們已經得到k時刻的最優溫度值了，下一步就是要進入 k+1時刻，進行新的最優估算。到現在為止，好像還沒看到什么自回歸的東西出現。對了，在進入k+1時刻之前，我們還要算出k時刻那個最優值（24.56 度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35（該式對應（5）式）。這里的5就是上面的k時刻你預測的那個23度溫度值的偏差，得出的2.35就是進入 k+1時刻以后k時刻估算出的最優溫度值的偏差（對應於上面的3）。就是這樣，卡爾曼濾波器就不斷的把 covariance遞歸，從而估算出最優的溫度值。他運行的很快，而且它只保留了上一時刻的covariance。上面的Kg，就是卡爾曼增益（Kalman Gain）。他可以隨不同的時刻而改變他自己的值，是不是很神奇！
我覺得應該我的標注應該能讓大家看懂這個例子了解整個迭代計算的過程了，下面我們看看程序吧，這個是一個比較簡單的波形跟隨，希望能給大家一點啟示。
%第一條曲線
% t=0.1:0.1:6;
% X=t.^2;
%第二條曲線
% t=1:1:60;
% X=t.^2;
%第三條曲線
t=1:0.5:60;
X=3*sin(t);
% %第四條曲線
% t=1:0.5:60;
% X=sin(t);
%N=1:60;
figure;
subplot(2,1,1);
plot(1:0.5:60,X,'r');
axis([0 60 -5 5])
hold on;
%系統方程：X(k+1)=A*X(k)+w(k)
%觀測方程：Z(k)=H*Z(k)+v(k)
%這個是生成高斯噪聲的隨機數
w=randn(1,length(t));
v=randn(1,length(t));
A=1;
H=1;
X_k(1)=0; %狀態估計初值
P_kk(1)=0; %P（k/k）
P_k(1)=0; %P（k/k-1） 這個初始化不需要，就給你們看看變量的對應
Z_k(1)=X_k(1)+w(1) ; %測量值
R=(std(v)).^2;
Q=(std(w)).^2;
Kg(1)=P_kk(1)*H'/(H*P_kk(1)*H'+R); %卡爾曼增益 Kg
P_k(1)=A*P_kk(1)*A'+Q ; %方差預測 P_k/k-1
for i=2:length(t)
Z_k(i)=H*X(i)+w(i) ;
Kg(i)=P_k(i-1)*H'/((H*P_k(i-1)*H'+R)) ;
P_k(i)=A*P_kk(i-1)*A'+Q;
P_kk(i)=P_k(i-1)-Kg(i)*H*P_kk(i-1);
X_k(i)=A*X_k(i-1)+Kg(i-1)*(Z_k(i)-H*A*X_k(i-1)); %這邊就直接代入式（1），所以沒出現
end
n=1:60;
plot(1:0.5:60,X_k);
subplot(2,1,2);
plot(1:0.5:60,Z_k);
axis([0 60 -5 5])
后續可能還有關於不同信號類型的卡爾曼濾波分析
（見http://blog.csdn.net/sillykog/article/details/78535767）
（本文章陳默含出品，轉載請注明出處。
文章鏈接：http://www.cnblogs.com/sillykog/p/6618587.html
作者：陳默含）