第一重：初見Kalman

假設我養了一隻豬：

一週前，這隻豬的體重是46±0.5kg。注意，在這裡我用了±0.5，表示其實我對這隻豬一週前的體重並不是那麼確定的，也就是說，46kg這個體重有0.5kg的誤差。現在，我又養了這隻豬一個星期。那麼我想要知道它一個星期之後多重，又大概有多少的誤差？

為了得到一週後的體重，我有兩種方法：一是根據我多年的養豬經驗得到的豬體重公式推求出一個大概的值，另一個就是直接去稱它的體重。當然，兩種方法都有一定的誤差。假設經驗公式得到的體重是48kg，誤差2kg；直接稱體重得到的是49kg，誤差1kg：

可是，我是一個處女座的人，不管是經驗公式得到的值，還是直接稱量得到的值，我都覺得不夠準。我希望有一種方法，可以同時結合這隻豬一週前的體重、用經驗公式估計的值以及直接稱量得到的值，綜合考慮，得出一個最接近豬真實體重的，誤差最小的值。這就是卡爾曼濾波要完成的任務。現在我們來把養豬的模型抽象成數學公式：

上圖的左邊，上一週的豬的體重，可以抽象為也k-1時刻的狀態值，用k-1時刻的最優估計值加上一個誤差項來表示，右邊同理。其中,

Pk=E[ekeTk]Pk=E[ekekT]

這一項表示的是估計值的協方差。這裡要說明兩點： 1. 上圖中所有的變數都是用粗體，表示這是一個向量或者一個矩陣； 2. 之所以用（列）向量而非一個數來表示狀態值，是因為，雖然一隻豬的體重可以用一個值來表示，但是在實際的應用中很多狀態並不是一個數就能表示的（比如導彈在空間中的位置，同時有x、y、z三個座標）。 圖中右邊表示k時刻的狀態值，這個值可以通過預測模組（也就是根據經驗公式估計豬的體重）和糾錯模組（也就是直接去稱量豬的體重值）來估計。同樣，預測模組和糾錯模組都有著對應的誤差和誤差協方差矩陣。卡爾曼濾波要做的，就是根據貝葉斯估計的相關理論，同時考慮預測模組和糾錯模組的協方差，對誤差小的項賦予較大的權重，對誤差大的項賦予較小的權重，並使預測的誤差最小。

具體的實現過程如下：

第二重：Kalman的數學原理

首先，我們先要引入一個離散控制過程的系統。該系統可用一個線性隨機微分方程（Linear Stochastic Difference equation）來描述：

上兩式子中，x(k)是k時刻的系統狀態，u(k)是k時刻對系統的控制量。A和B是系統引數，對於多模型系統，他們為矩陣。y(k)是k時刻的測量值，H是測量系統的引數，對於多測量系統，H為矩陣。q(k)和r(k)分別表示過程和測量的噪聲。他們被假設成高斯白噪聲(White Gaussian Noise)，他們的covariance分別是Q，R（這裡我們假設他們不隨系統狀態變化而變化）。

對於滿足上面的條件(線性隨機微分系統，過程和測量都是高斯白噪聲)，卡爾曼濾波器是最優的資訊處理器。下圖給出KF演算法的流程和五個核心更新方程如下：

五個更新方程為：

舉個栗子：

假設我們要研究的物件是一個房間的溫度。根據你的經驗判斷，這個房間的溫度是恆定的。（這裡的假設相當於狀態方程的係數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度，他們平方相加再開方，就是5（算出來的就是P(k|k-1)））。然後，你從溫度計那裡得到了k時刻的溫度值（測量值Z(k)），假設是25度，同時該值的偏差是4度。由於我們用於估算k時刻的實際溫度有兩個溫度值，分別是23 度和25度。究竟實際溫度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點，我們可以用他們的covariance（協方差）來判斷。因為 Kg^2=5^2/(5^2+4^2)所以Kg=0.78，我們可以估算出k時刻的實際溫度值是：23+0.78*(25-23)=24.56度。可以看出，因為溫度計的covariance比較小（比較相信溫度計），所以估算出的最優溫度值偏向溫度計的值。

現在我們已經得到k時刻的最優溫度值了，下一步就是要進入 k+1時刻，進行新的最優估算。到現在為止，好像還沒看到什麼自迴歸的東西出現。對了，在進入k+1時刻之前，我們還要算出k時刻那個最優值（24.56 度）的偏差。演算法如下：((1-Kg)*5^2)^0.5=2.35。這裡的5就是上面的k時刻你預測的那個23度溫度值的偏差，得出的2.35就是進入 k+1時刻以後k時刻估算出的最優溫度值的偏差。就是這樣，卡爾曼濾波器就不斷的把 covariance遞迴，從而估算出最優的溫度值。他執行的很快，而且它只保留了上一時刻的covariance。上面的Kg，就是卡爾曼增益（Kalman Gain）。他可以隨不同的時刻而改變他自己的值，是不是很神奇！

第三重：Kalman的數學推導

首先要計算預測值、預測值和真實值之間誤差協方差矩陣：

有了這兩個就能計算卡爾曼增益K，再然後得到估計值：

最後還要計算估計值和真實值之間的誤差協方差矩陣，為下次遞推做準備：

具體原理推導過程參考：

《影象處理、分析與機器視覺》第四版16.6.1 卡爾曼濾波器