理论部分：
卡尔曼滤波器,是一个最优化自回归数据处理算法。简单的说就是通过现有测量值，来预测最优值。
网上的一些教程上来就是一大堆推导公式，很是羞涩难懂。我们先说个例子，来了解：
假设我们要研究的对象是一个房间的温度。
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