轉自點擊打開鏈接
卡爾曼濾波器算法
（The Kalman Filter Algorithm）
在這一部分，我們就來描述源於Dr Kalman 的卡爾曼濾波器。下面的描述，會涉及一些基本的概念知識，包括概率（Probability），隨即變量（Random Variable），高斯或正態分配（Gaussian Distribution）還有State-space Model等等。但對於卡爾曼濾波器的詳細證明，這裏不能一一描述。
首先，我們先要引入一個離散控制過程的系統。該系統可用一個線性隨機微分方程（Linear
Stochastic Difference equation）來描述：
X(k)=A X(k-1)+B U(k)+W(k)
再加上系統的測量值：
Z(k)=H X(k)+V(k)
上兩式子中，X(k)是k時刻的系統狀態，U(k)是k時刻對系統的控制量。A和B是系統參數，對於多模型系統，他們爲矩陣。Z(k)是k時刻的測量值，H是測量系統的參數，對於多測量系統，H爲矩陣。W(k)和V(k)分別表示過程和測量的噪聲。他們被假設成高斯白噪聲(White
Gaussian Noise)，他們的covariance 分別是Q，R（這裏我們假設他們不隨系統狀態變化而變化）。
對於滿足上面的條件(線性隨機微分系統，過程和測量都是高斯白噪聲)，卡爾曼濾波器是最優的信息處理器。下面我們來用他們結合他們的covariances 來估算系統的最優化輸出（類似上一節那個溫度的例子）。
首先我們要利用系統的過程模型，來預測下一狀態的系統。假設現在的系統狀態是k，根據系統的模型，可以基於系統的上一狀態而預測出現在狀態：
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)
式(1)中，X(k|k-1)是利用上一狀態預測的結果，X(k-1|k-1)是上一狀態最優的結果，U(k)爲現在狀態的控制量，如果沒有控制量，它可以爲0。
到現在爲止，我們的系統結果已經更新了，可是，對應於 X(k|k-1)的covariance(協方差)還沒更新。我們用P表示covariance：
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
式(2)中，P(k|k-1)是X(k|k-1)對應的covariance，P(k-1|k-1)是X(k-1|k-1)對應的 covariance，A’表示A的轉置矩陣，Q是系統過程的covariance。式子1，2就是卡爾曼濾波器5個公式當中的前兩個，也就是對系統的預測。
現在我們有了現在狀態的預測結果，然後我們再收集現在狀態的測量值。結合預測值和測量值，我們可以得到現在狀態(k)的最優化估算值X(k|k)：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
其中Kg爲卡爾曼增益(Kalman Gain)：
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
到現在爲止，我們已經得到了k狀態下最優的估算值 X(k|k)。但是爲了要另卡爾曼濾波器不斷的運行下去直到系統過程結束，我們還要更新k狀態下X(k|k)的covariance：
P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)
其中I 爲1的矩陣，對於單模型單測量，I=1。當系統進入k+1狀態時，P(k|k)就是式子(2)的P(k-1|k-1)。這樣，算法就可以自迴歸的運算下去。
卡爾曼濾波器的原理基本描述了，式子1，2，3，4和5就是他的5 個基本公式。根據這5個公式，可以很容易的實現計算機的程序。
下面，用Matlab程序舉一個實際運行的例子。
4． 簡單例子
（A Simple Example）
這裏我們結合第二第三節，舉一個非常簡單的例子來說明卡爾曼濾波器的工作過程。所舉的例子是進一步描述第二節的例子，而且還會配以程序模擬結果。
根據第二節的描述，把房間看成一個系統，然後對這個系統建模。當然，我們見的模型不需要非常地精確。我們所知道的這個房間的溫度是跟前一時刻的溫度相同的，所以A=1。沒有控制量，所以U(k)=0。因此得出：
X(k|k-1)=X(k-1|k-1) ……….. (6)
式子（2）可以改成：
P(k|k-1)=P(k-1|k-1) +Q ……… (7)
因爲測量的值是溫度計的，跟溫度直接對應，所以H=1。式子 3，4，5可以改成以下：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)
Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)
P(k|k)=（1-Kg(k)）P(k|k-1) ……… (10)
現在我們模擬一組測量值作爲輸入。假設房間的真實溫度爲25 度，我模擬了200個測量值，這些測量值的平均值爲25度，但是加入了標準偏差爲幾度的高斯白噪聲（在圖中爲藍線）。
爲了令卡爾曼濾波器開始工作，我們需要告訴卡爾曼兩個零時刻的初始值，是X(0|0)和P(0|0)。他們的值不用太在意，隨便給一個就可以了，因爲隨着卡爾曼的工作，X會逐漸的收斂。但是對於
P，一般不要取0，因爲這樣可能會令卡爾曼完全相信你給定的X(0|0)是系統最優的，從而使算法不能收斂。我選了X(0|0)=1 度，P(0|0)=10。
該系統的真實溫度爲25度，圖中用黑線表示。圖中紅線是卡爾曼濾波器輸出的最優化結果（該結果在算法中設置了Q=1e-6，R=1e-1）。
matlab仿真代碼
clear
clc;
N=300;
CON = 25;%房間溫度，假定溫度是恆定的
%%%%%%%%%%%%%%%kalman filter%%%%%%%%%%%%%%%%%%%%%%
x = zeros(1,N);
%randn(1,N)
y = 2^0.5 * randn(1,N)*0.5 + CON;%加過程噪聲的狀態輸出
x(1) = 1;
p = 10;
cov(randn(1,N))
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
%%%%%%%%%%%Smoothness Filter%%%%%%%%%%%%%%%%%%%%%%%%
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');