理論部分：
卡爾曼濾波器,是一個最優化自回歸資料處理算法。簡單的說就是通過現有測量值，來預測最優值。
網上的一些教程上來就是一大堆推導公式，很是羞澀難懂。我們先說個例子，來了解：
假設我們要研究的對象是一個房間的溫度。
1.根據經驗，溫度是恒定的，即上一分鐘的溫度等于現在這一分鐘的溫度，經驗即預測，但這并不是完全可信的，即存在一定的誤差。我們假設成高斯白噪聲。
2.另外在房間裡放一個溫度計，實時檢測房間的溫度，這是觀測值。同樣地也存在誤差，也是高斯白噪聲。
我們現在的目标就是，用這兩個值，結合它們各自的噪聲，估算出房間的實際溫度。假設要估算k時刻的實際溫度值，首先需要知道k-1時刻的溫度值。
（1）預測值，假設k-1時刻房間溫度為23度，則k時刻的溫度應該也為23度（恒定）。同時該值的偏差為5（5是3的平方加上4的平方再開方，其中3為k-1時刻估算出的最優溫度值的偏差3，4為測量的偏差）。
（2）測量值，假設為25度，同時偏差為4度。
那麼實際值為多少？相信誰多一點，可用它們的協方差來判斷：
則實際的溫度值是：23+0.78*(25-23) = 24.56度。可以看出溫度計測量的covariance較小，是以實際值比較偏向溫度計。
在進入k+1時刻估算前，需要算出k時刻最優值（24.56）的偏差。算法
，5即k時刻估算所用k-1時刻23度溫度值的偏差。得出的2.35即k時刻最優值的偏差（對應上面k-1時刻的3）。
這樣卡爾曼濾波器就不斷得把covariance遞歸，進而估算出最優值。而且它隻保留上一刻的covariance。上面的Kg就是卡爾曼增益（Kalman Gain）。
将該例子用數學公式來描述：
首先引入一個離散控制過程的系統，改系統可用一個線性随機微分方程（linear stochastic difference equation）來描述：
再加上系統的測量值：
其中：X(k)是k時刻的系統狀态，U(k)是k時刻對系統的控制量。A和B是系統參數，對于多模系統，它們為矩陣。
Z(k)是k時刻的測量值，H是測量參數，對于多模系統，H為矩陣。W(k)和V(k)分别表示過程和測量的噪聲，它們被假設成高斯白噪聲，它們的covariance分别是Q，R。
對于滿足上面條件（線性随機微分系統，過程和測量都是高斯白噪聲），卡爾曼濾波器是最優的資訊處理器。
卡爾曼濾波器算法流程及核心公式
首先利用系統過程模型，來預測下一個狀态的系統：
X(k | k-1) 是利用上一狀态預測的結果，X(k-1 | k-1) 是上一狀态最優結果。U(k)為現在狀态的控制量，若沒有，可為0.
現在更新 X(k | k-1) 的covariance，用P表示：
其中 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各公式的前兩個，用來對系統進行預測。
然後再收集系統的觀測值 Z(k)，則最優值：
其中Kg為卡爾曼增益（Kalman Gain）:
到目前為止，已經得到k狀态下最優的估算值 X(k | k-1)。
但是為了要卡爾曼濾波器不斷得運作下去直到系統過程結束，需要更新k狀态下 X(k | k-1) 的covariance：
其中I為1的矩陣，對于單模系統I = 1。
當系統進入k+1狀态時，P(k | k) 就是式(2)中的 P(k-1 | k-1)。這樣，算法就可以自回歸地運算下去。
C#程式實作
/*
% Kalman filter example of temperature measurement in Matlab implementation of Kalman filter algorithm.
% 房間目前溫度真實值為1.5度，認為下一時刻與目前時刻溫度相同，誤差為0.02度（即認為連續的兩個時刻最多變化0.02度）。
% 溫度計的測量誤差為0.5度。
% 開始時，房間溫度的估計為1.2度，誤差為0.5度。
*/
定義變量：
int N = 5000; //測試資料個數
float x = 1.5f; //溫度的真實值
float Q = 0.0004f; //過程方差。 （連續兩個時刻溫度方差）
float R = 0.25f; //測量方差。 （測量誤差的平方）
float Z = 0.0f; //測量值-在真實值的基礎上加上了方差為0.25的高斯噪聲。
float X_First = 0.0f; //溫度計的先驗估計
float P_First = 0.0f; //溫度計的先驗方差
float X_End = 1.2f; //溫度計的後驗估計
float P_End = 0.5f; //溫度計的後驗方差
float K = 0.0f; //卡爾曼增益,平方
計算：
private void KaermanCalc()
{
//測量值-在真實值的基礎上加上了方差為0.25的高斯噪聲。
Z = x + (float)Math.Sqrt(R) * rd.Next(-100, 100) / 20;
//卡爾曼最優值估計
X_First = X_End;
P_First = P_End + Q;
K = P_First / (P_First + R);
X_End = X_First + K * (Z - X_First);
P_End = (1 - K) * P_First;
}
最終實作效果：
完整工程代碼連接配接：
https://download.csdn.net/download/panjinliang066333/12376929