理論部分：
卡爾曼濾波器,是一個最優化自迴歸數據處理算法。簡單的說就是通過現有測量值，來預測最優值。
網上的一些教程上來就是一大堆推導公式，很是羞澀難懂。我們先說個例子，來了解：
假設我們要研究的對象是一個房間的溫度。
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）的偏差。算法
這樣卡爾曼濾波器就不斷得把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