Kalman Filter 簡介

Kalman 濾波器的部份，我發現網路上有一些不錯的介紹 (無法找到原作者 )，應用一引用其一。

在學習 Kalman 濾波器之前，首先看看為什麼叫「Kalman」。跟其他著名的理論（例如傅立葉變換，泰勒級數等等）一樣，Kalman也是一個人的名字，不同的是，他是個二十世紀的人。

Kalman 全名 Rudolf Emil Kalman，匈牙利數學家，1930年出生於匈牙利首都布達佩斯。1953，1954年於麻省理工學院分別獲得電機工程學士及碩士學位。1957年於哥倫比亞大學獲得博士學位。我們現在要學習的Kalman濾波器，正是源於他的博士論文和1960年發表的論文《A New Approach to Linear Filtering and Prediction Problems》（線性濾波與預測問題的新方法）。如果對這編論文有興趣，可以到這裡的地址下載： http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf

故名思義，Kalman 濾波器有兩類應用：一類是濾波 (filtering, 或稱為 smoothing)，另一類是預測 (prediction)。特別後者被廣泛用在需要預測+修正 (prediction + correction) 的場合。簡單來說，Kalman濾波器是一個「optimal recursive data processing algorithm（最佳化自回歸數據處理演算法）」。對於解決很大部分的問題，他是最優，效率最高甚至是最有用的。他的廣泛應用已經超過30年，包括機器人導航，控制，傳感器數據融合甚至在軍事方面的雷達系統以及導彈追蹤等等。近年來更被應用於計算機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等。

Kalman濾波器的介紹

為了可以更加容易的理解Kalman濾波器，這裡會用定性的描述方法來解釋，而不是像大多數參考書那樣羅列一大堆的數學公式和數學符號。雖然 Kalman filter 的5條公式是其核心內容，非常適合電腦執行。其實 Kalman 的程序相當的簡單，只要你理解了他的那5條公式。 在介紹他的5條公式之前，先讓我們來根據下面的例子一步一步的探索。

假設我們要研究的對象是一個房間的溫度。根據你的經驗判斷，這個房間的溫度是恆定的，也就是下一分鐘的溫度等於現在這一分鐘的溫度（假設我們用一分鐘來做時間單位）。假設你對你的經驗不是100%的相信，可能會有上下偏差幾度。我們把這些偏差看成是高斯白噪聲（White Gaussian Noise），也就是這些偏差跟前後時間是沒有關係的而且符合高斯分配（Gaussian Distribution）。另外，我們在房間裡放一個溫度計，但是這個溫度計也不準確的，測量值會比實際值偏差。我們也把這些偏差看成是高斯白噪聲。

好了，現在對於某一分鐘我們有兩個有關於該房間的溫度值：你根據經驗的預測值（系統的預測值）和溫度計的值（測量值）。下面我們要用這兩個值結合他們各自的噪聲來估算出房間的實際溫度值。

假如我們要估算k時刻的是實際溫度值。首先你要根據k-1時刻的溫度值，來預測k時刻的溫度。因為你相信溫度是恆定的，所以你會得到k時刻的溫度預測值是跟 k-1時刻一樣的，假設是23度，同時該值的高斯噪聲的偏差是5度（5是這樣得到的：如果k-1時刻估算出的最優溫度值的偏差是3，你對自己預測的不確定度是4度，他們平方相加再開方，就是5）(Step 1 and 2, state and covariance prediction)。然後，你從溫度計那裡得到了k時刻的溫度值，假設是25度，同時該值的偏差是4度。

由於我們用於估算k時刻的實際溫度有兩個溫度值，分別是23度和25度。究竟實際溫度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點，我們可以用他們的 covariance來判斷。因為Kg^2=5^2/(5^2+4^2)，所以Kg=0.78 (Step 4, Kalman gain correction)，我們可以估算出k時刻的實際溫度值是：23+0.78* (25-23)=24.56度 (Step 3, state correction)。可以看出，因為溫度計的covariance比較小（比較相信溫度計），所以估算出的最優溫度值偏向溫度計的值。

現在我們已經得到k時刻的最優溫度值了，下一步就是要進入k+1時刻，進行新的最優估算。到現在為止，好像還沒看到什麼自回歸的東西出現。對了，在進入 k+1時刻之前，我們還要算出k時刻那個最優值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35 (Step 5, covariance correction)。這裡的5就是上面的k時刻你預測的那個23度溫度值的偏差，得出的2.35就是進入k+1時刻以後k時刻估算出的最優溫度值的偏差（對應於上面的3）。

就是這樣，Kalman濾波器就不斷的把covariance遞歸，從而估算出最優的溫度值。他運行的很快，而且它只保留了上一時刻的covariance。上面的Kg，就是Kalman增益（Kalman Gain）。他可以隨不同的時刻而改變他自己的值，是不是很神奇！

下面就要言歸正傳，討論真正工程系統上的Kalman。

Kalman濾波器演算法

在這一部分，我們就來描述源於 Dr Kalman 的Kalman濾波器。下面的描述，會涉及一些基本的概念知識，包括機率（Probability），隨機變量（Random Variable），高斯或常態分配（Gaussian Distribution）還有State-space Model等等。但對於Kalman濾波器的詳細證明，這裡不能一一描述。

首先，我們先要引入一個離散控制過程的系統。該系統可用一個線性隨機微分方程（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（這裡我們假設他們不隨系統狀態變化而變化）。

對於滿足上面的條件(線性隨機微分系統，過程和測量都是高斯白噪聲)，Kalman濾波器是最佳的信息處理器。下面我們來用他們結合他們的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就是Kalman濾波器5個公式當中的前兩個，也就是對系統的預測。
現在我們有了現在狀態的預測結果，然後我們再收集現在狀態的測量值。結合預測值和測量值，我們可以得到現在狀態(k)的最優化估算值X(k|k)：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
其中Kg為Kalman增益(Kalman Gain)：
Kg(k)= P(k|k-1) H' / (H P(k|k-1) H' + R) ……… (4)
到現在為止，我們已經得到了k狀態下最優的估算值X(k|k)。但是為了要另Kalman濾波器不斷的運行下去直到系統過程結束，我們還要更新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)。這樣，算法就可以自回歸的運算下去。
Kalman濾波器的原理基本描述了，式子1，2，3，4和5就是他的5 個基本公式。根據這5個公式，可以很容易的實現計算機的程序。
下面，我會用程序舉一個實際運行的例子。。。 
 

應用一： 找出 (estimate) 室溫

這裡我們舉一個非常簡單的例子來說明 Kalman 濾波器的工作過程。所舉的例子是進一步描述上一節的例子，而且還會配以 matlab 模擬結果。

根據第二節的描述，把房間看成一個系統，然後對這個系統建模。當然，我們見的模型不需要非常地精確。我們所知道的這個房間的溫度是跟前一時刻的溫度相同的，所以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度，但是加入了標準偏差為幾度的高斯白噪聲（在圖中為藍線）。

為了令Kalman濾波器開始工作，我們需要告訴Kalman兩個零時刻的初始值，是X(0|0)和P(0|0)。他們的值不用太在意，隨便給一個就可以了，因為隨著Kalman的工作，X會逐漸的收斂。但是對於P，一般不要取0，因為這樣可能會令Kalman完全相信你給定的X(0|0)是系統最優的，從而使算法不能收斂。我選了 X(0|0)=1度，P(0|0)=10。

該系統的真實溫度為25度，圖中用黑線表示。圖中紅線是Kalman濾波器輸出的最優化結果（該結果在算法中設置了Q=1e-6，R=1e-1）。

附matlab下面的kalman濾波程序：

clear

N=200;

w(1)=0;

w=randn(1,N)

x(1)=0;

a=1;

for k=2:N;

x(k)=a*x(k-1)+w(k-1);

end

V=randn(1,N);

q1=std(V);

Rvv=q1.^2;

q2=std(x);

Rxx=q2.^2;

q3=std(w);

Rww=q3.^2;

c=0.2;

Y=c*x+V;

p(1)=0;

s(1)=0;

for t=2:N;

p1(t)=a.^2*p(t-1)+Rww;

b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);

s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));

p(t)=p1(t)-c*b(t)*p1(t);

end

t=1:N;

plot(t,s,'r',t,Y,'g',t,x,'b');

 

 應用二：Symbol(Bit) Synchronization with Unknown Phase/Frequency

考慮如下一個 burst mode 的 packet (任何 shared media 的傳送方式)。在 packet 最前頭都會有 1010.. 的 preamble 做為 bit/symbol synchronization。緊接在後的是事先定義好的 frame sync word 用來做為確認 lock 到對的 packet 以及 word boundary。最後才是實際的 data。這個應用 focus 在 preamble 部份 (1010..) synchronization，也就是找出 (estimate) preamble 的相位和頻率，同步之後的 data。

 

我們可以把問題重新定義如下：A [2pft + q]  如何找出 f (or b = 1/(2pf) or T=1/f) and q ?

這個問題同樣可以用 state-space model 來模擬，而得到最佳的 Kalman filter 解。有趣的是，Kalman filter 等價於一個 2nd order time variant PLL 架構。這和一般所用 2nd order time invariant PLL 非常近似。可參見另文 Kalman Filter and PLL。

應用三：TCP/IP 演算法和 Kalman Filter