卡爾曼濾波器(Kalman Filter) 說明與介紹
1． 什麼是卡爾曼濾波器（What is the Kalman Filter?）
在學習卡爾曼濾波器之前，首先看看為什麼叫“卡爾曼”。跟其他著名的理論（例如傅立葉變換，泰勒級數等等）一樣，卡爾曼也是一個人的名字，而跟他們不同的是，他是個現代人！
卡爾曼全名Rudolf Emil Kalman，匈牙利數學家，1930年出生於匈牙利首都布達佩斯。1953，1954年于麻省理工學院分別獲得電機工程學士及碩士學位。1957年于哥倫比亞大學獲得博士學位。我們現在要學習的卡爾曼濾波器，正是源于他的博士論文和1960年發表的論文《A New Approach to Linear Filtering and Prediction Problems》（線性濾波與預測問題的新方法）。如果對這編論文有興趣，可以到這裏的位址下載： http://www.dsprelated.com/showabstract/16.php。
簡單來說，卡爾曼濾波器是一個“optimal recursive data processing algorithm（最優化自回歸資料處理演算法）”。對於解決很大部分的問題，他是最優，效率最高甚至是最有用的。他的廣泛應用已經超過30年，包括機器人導航，控制，感測器資料融合甚至在軍事方面的雷達系統以及導彈追蹤等等。近年來更被應用於電腦圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等。
2．卡爾曼濾波器的介紹（Introduction to the Kalman Filter）
為了可以更加容易的理解卡爾曼濾波器，這裏會應用形象的描述方法來講解，而不是像大多數參考書那樣羅列一大堆的數學公式和數學符號。但是，他的5條公式是其核心內容。結合現代的電腦，其實卡爾曼的程式相當的簡單，只要你理解了他的那5條公式。
在介紹他的5條公式之前，先讓我們來根據下面的例子一步一步的探索。
假設我們要研究的是一個房間的溫度，根據經驗判斷，這個房間的溫度是恆溫的。也就是下一分鐘的溫度會等於現在這一分鐘的溫度（這裡設定是以一分鐘為時間單位）。
但事實上，房間不是恆溫的，而且以你經驗估出來的溫度，可能會有上下幾度的誤差，這些誤差就是所謂的高斯白雜訊（White Gaussian Noise），這些誤差跟前後時間是沒有關係的而且符合高斯分配（Gaussian Distribution）。
另外，我們在房間裏也放一支溫度計，但這溫度計也不是準確的，測量值和實際值也存在著誤差。這些誤差也是高斯白雜訊（White Gaussian Noise）。
好了，現在對於某一分鐘內我們有兩個關於該房間的溫度值：
1. 根據你經驗的預測值（系統的預測值）和
2. 溫度計的值（測量值）。
下面我們要用這兩個值結合他們各自的雜訊來估算出房間的實際溫度值。
（PS: 注意，這個估算出來的實際溫度也會有誤差，但隨著時間往後推移，每次算出來的實際溫度的誤差會越來越小！)
計算過程：
經驗法則預測的誤差 和 溫度計的誤差為 已知的系統參數
經驗法則預測的誤差 和 溫度計的誤差為 已知的系統參數
Step 1 , state and covariance prediction
假設我們要估算時間K的實際溫度，首先我們根據K-1時刻的溫度，來預測K時刻的溫度。
假設K-1時刻時，房間溫度是23度，我們預測K時刻房間溫度也是23度，K時刻預測的誤差為5度。
這邊的5度是如何得知的呢? 如果K-1時刻實際溫度的誤差是3，經驗法則預估的誤差為4，那麼將他們平方相加在開根號，就是K時刻預測的誤差。
然後再去看我們溫度計量出來的結果，假設為25度，同時該溫度計的誤差是4度。
Step 2 , Kalman gain correction and state correction
我們預測房間溫度是23度，測量到的溫度是25度，那麼實際溫度到底是多少呢?該相信哪邊多一點呢? 這邊就要看Kalman gain（Kg）了。
Kalman gain的計算要用到前面的兩個誤差
假設我們要估算時間K的實際溫度，首先我們根據K-1時刻的溫度，來預測K時刻的溫度。
假設K-1時刻時，房間溫度是23度，我們預測K時刻房間溫度也是23度，K時刻預測的誤差為5度。
這邊的5度是如何得知的呢? 如果K-1時刻實際溫度的誤差是3，經驗法則預估的誤差為4，那麼將他們平方相加在開根號，就是K時刻預測的誤差。
然後再去看我們溫度計量出來的結果，假設為25度，同時該溫度計的誤差是4度。
Step 2 , Kalman gain correction and state correction
我們預測房間溫度是23度，測量到的溫度是25度，那麼實際溫度到底是多少呢?該相信哪邊多一點呢? 這邊就要看Kalman gain（Kg）了。
Kalman gain的計算要用到前面的兩個誤差
Kg^2 = 5^2 / (5^2 + 4^2) = 0.78
由Kalman gain的值，我們可以推算出K時刻的實際溫度是
由Kalman gain的值，我們可以推算出K時刻的實際溫度是
預測值 + Kalman gain * (量測值 - 預測值)
23 + 0.78* (25 -23) = 24.56
這邊可以看出，因為溫度計的誤差較K時刻預測的誤差小(比較相信溫度計)，所以實際溫度偏向溫度計的值
Step 3 , covariance correction
但是這個實際溫度還是有誤差，可由以下式子算出K時刻實際溫度的誤差
((1-Kg)*5^2)^0.5=2.35這裡的5是K時刻預測的誤差，算出來的2.35對應到前面的3 (K-1時刻的實際溫度誤差)現在我們已經得到k時刻的最優溫度值了，接下來我們就可以進入K+1時刻了！
根據以上規則，Kalman filter一直迭代下去，
我們可以發現實際溫度的誤差(covariance)會慢慢的變小，我們得到的實際溫度也會越來越準確！
下面就要言歸正傳，討論真正工程系統上的卡爾曼。
3． 卡爾曼濾波器演算法（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，根據系統的模型，可以基於系統的上一狀態而預測出現在狀態：
首先我們要利用系統的過程模型，來預測下一狀態的系統。假設現在的系統狀態是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：
到現在為止，我們的系統結果已經更新了，可是，對應於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)：
現在我們有了現在狀態的預測結果，然後我們再收集現在狀態的測量值。結合預測值和測量值，我們可以得到現在狀態(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：
到現在為止，我們已經得到了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個公式，可以很容易的實現電腦的程式。
下面，我會用程式舉一個實際運行的例子。
卡爾曼濾波器的原理基本描述了，式子1，2，3，4和5就是他的5 個基本公式。根據這5個公式，可以很容易的實現電腦的程式。
下面，我會用程式舉一個實際運行的例子。
4． 簡單例子 （A Simple Example）
這裏我們結合第二第三節，舉一個非常簡單的例子來說明卡爾曼濾波器的工作過程。所舉的例子是進一步描述第二節的例子，而且還會配以程式類比結果。
根據第二節的描述，把房間看成一個系統，然後對這個系統建模。當然，我們見的模型不需要非常地精確。我們所知道的這個房間的溫度是跟前一時刻的溫度相同的，所以A=1。沒有控制量，所以U(k)=0。因此得出：
X(k|k-1)=X(k-1|k-1) ……….. (6)
式子（2）可以改成：
式子（2）可以改成：
P(k|k-1)=P(k-1|k-1) +Q ……… (7)
因為測量的值是溫度計的，跟溫度直接對應，所以H=1。式子3，4，5可以改成以下：
因為測量的值是溫度計的，跟溫度直接對應，所以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)
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）。
為了令卡爾曼濾波器開始工作，我們需要告訴卡爾曼兩個零時刻的初始值，是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）。
refer to: http://rexkingworld.blogspot.tw/2013/05/kalman-filter.html
沒有留言:
張貼留言