卡尔曼滤波

将预测值和测量值进行结合，对系统状态进行最优估计的算法。

在连续变化的系统中使用卡尔曼滤波是非常理想的，它具有占用内存小的优点（除了前一个状态量外，不需要保留其它历史数据），并且速度很快，很适合应用于实时问题和嵌入式系统。

根据k-1时刻的系统状态预测k时刻系统状态。

k时刻系统状态，匀速变化假设
k时刻系统状态各维度的协方差矩阵
矩阵表示

考虑外部因素控制的影响
外部因素会对系统进行控制，从而带来一些与系统自身状态没有相关性的改变。其中成为控制矩阵， 称为控制向量，如果没有外部控制，这部分可以忽略。

引入外部控制因素

外部噪声因素
在每次预测之后，我们可以添加一些新的不确定性来建立这种与“外界”（即我们没有跟踪的干扰）之间的不确定性模型

引入外部不可控的噪声

小结：
由上两式可知，新的最优估计是根据上一最优估计预测得到的，并加上已知外部控制量的修正。 而新的不确定性由上一不确定性预测得到，并加上外部环境的干扰。

加入传感器观测数据
卡尔曼滤波的一大优点就是能处理传感器噪声，我们的传感器或多或少都有点不可靠，并且原始估计中的每个状态可以和一定范围内的传感器读数对应起来。 从测量到的传感器数据中，我们大致能猜到系统当前处于什么状态。但是由于存在不确定性，某些状态可能比我们得到的读数更接近真实状态。
传感器早上用协方差表示，该分布的均值是我们读取到的传感器数据。
于是我们得到两个高斯分布，一个是预测值附近，一个是传感器读数附近。把两个具有不同均值和方差的高斯分布相乘，得到一个新的具有独立均值和方差的高斯分布。
结果如下，其中，K为卡尔曼增益。

两个高斯分布
两个高斯分布相乘结果
更新系统状态

总结：
我们可以用这些公式对任何线性系统建立精确的模型，对于非线性系统来说，我们使用扩展卡尔曼滤波，区别在于EKF多了一个把预测和测量部分进行线性化的过程。

卡尔曼滤波

参考文章：
http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
https://blog.csdn.net/u010720661/article/details/63253509