- 初見Kalman
假設我養了一只豬：
一周前，這只豬的體重是46±0.5kg。注意，在這里我用了±0.5，表示其實我對這只豬一周前的體重并不是那么確定的，也就是說，46kg這個體重有0.5kg的誤差。現在，我又養了這只豬一個星期。那么我想要知道它一個星期之后多重，又大概有多少的誤差？
為了得到一周后的體重，我有兩種方法：一是根據我多年的養豬經驗得到的豬體重公式推求出一個大概的值，另一個就是直接去稱它的體重。當然，兩種方法都有一定的誤差。假設經驗公式得到的體重是48kg，誤差2kg；直接稱體重得到的是49kg，誤差1kg：為了得到一周后的體重，我有兩種方法：一是根據我多年的養豬經驗得到的豬體重公式推求出一個大概的值，另一個就是直接去稱它的體重。當然，兩種方法都有一定的誤差。假設經驗公式得到的體重是48kg，誤差2kg；直接稱體重得到的是49kg，誤差1kg：
可是，我是一個處女座的人，不管是經驗公式得到的值，還是直接稱量得到的值，我都覺得不夠準。我希望有一種方法，可以同時結合這只豬一周前的體重、用經驗公式估計的值以及直接稱量得到的值，綜合考慮，得出一個最接近豬真實體重的，誤差最小的值。這就是卡爾曼濾波要完成的任務。現在我們來把養豬的模型抽象成數學公式：
上圖的左邊，上一周的豬的體重，可以抽象為也k-1時刻的狀態值，用k-1時刻的最優估計值加上一個誤差項來表示，右邊同理。其中，
這一項表示的是估計值的協方差。這里要說明兩點：
- 上圖中所有的變量都是用粗體，表示這是一個向量或者一個矩陣；
- 之所以用（列）向量而非一個數來表示狀態值，是因為，雖然一只豬的體重可以用一個值來表示，但是在實際的應用中很多狀態并不是一個數就能表示的（比如導彈在空間中的位置，同時有x、y、z三個坐標）。
圖中右邊表示k時刻的狀態值，這個值可以通過預測模塊（也就是根據經驗公式估計豬的體重）和糾錯模塊（也就是直接去稱量豬的體重值）來估計。同樣，預測模塊和糾錯模塊都有著對應的誤差和誤差協方差矩陣。卡爾曼濾波要做的，就是根據貝葉斯估計的相關理論，同時考慮預測模塊和糾錯模塊的協方差，對誤差小的項賦予較大的權重，對誤差大的項賦予較小的權重，并使預測的誤差最小。
具體的實現過程如下：
參考：
https://www.zhihu.com/question/23971601/answer/137325095
- Kalman的數學原理
假設我們要研究的對象是一個房間的溫度。根據你的經驗判斷，這個房間的溫度是恒定的。（這里的假設相當于狀態方程的系數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度（這個就是R了）。由于我們用于估算k時刻的實際溫度有兩個溫度值，分別是23 度和25度。究竟實際溫度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點，我們可以用他們的covariance（協方差）來判斷。因為 Kg2=52/(52+42)(該式的計算相當于上面的（4）式子)，所以Kg=0.78，我們可以估算出k時刻的實際溫度值是：23+0.78*(25-23)=24.56度（該式對應（3）式，就是算出最優估算值）。可以看出，因為溫度計的covariance比較小（比較相信溫度計），所以估算出的最優溫度值偏向溫度計的值。
現在我們已經得到k時刻的最優溫度值了，下一步就是要進入 k+1時刻，進行新的最優估算。到現在為止，好像還沒看到什么自回歸的東西出現。對了，在進入k+1時刻之前，我們還要算出k時刻那個最優值（24.56 度）的偏差。算法如下：((1-Kg)*52)0.5=2.35（該式對應（5）式）。這里的5就是上面的k時刻你預測的那個23度溫度值的偏差，得出的2.35就是進入 k+1時刻以后k時刻估算出的最優溫度值的偏差（對應于上面的3）。就是這樣，卡爾曼濾波器就不斷的把 covariance遞歸，從而估算出最優的溫度值。他運行的很快，而且它只保留了上一時刻的covariance。上面的Kg，就是卡爾曼增益（Kalman Gain）。他可以隨不同的時刻而改變他自己的值，是不是很神奇！
