首先，我們引入一個離散控制過程的系統。該系統可用一個線性隨機微分方程來描述：
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)式的預測方程得到的，你看他乘的係數也是A就懂了）
其中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）。他可以隨不同的時刻而改變他自己的值，是不是很神奇！（自學了好久菜看懂這個例子是什麼意思）
我覺得應該我的標註應該能讓大家看懂這個例子瞭解整個迭代計算的過程了，下面我們看看程序吧，這個是一個比較簡單的波形跟隨，希望能給大家一點啓示。
1
%第一條曲線
% 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])
後續可能還有關於不同信號類型的卡爾曼濾波分析