文档介绍：卡尔曼滤波
前言
卡尔曼滤波器是在估计线性系统状态的过程中，以最小均方差为目的而推导出的几个递推数学等式,也可以从贝叶斯推断的角度来推导。
本文将分为两部分：
第一部分，结-------------------------------------------- 这部分每讲一个数学性的东西，接着就会有相应的例子和直观的分析帮助理解。
首先假设我们知道一个线性系统的状态差分方程为

其中x是系统的状态向量，大小为n*1列。A为转换矩阵，大小为n*n。u为系统输入，大小为k*1。B是将输入转换为状态的矩阵，大小为n*k。随机变量w为系统噪声。注意这些矩阵的大小，它们与你实际编程密切相关。
看一个具体的匀加速运动的实例。
有一个匀加速运动的小车，它受到的合力为 ft , 由匀加速运动的位移和速度公式，能得到由 t-1 到 t 时刻的位移和速度变化公式：

该系统系统的状态向量包括位移和速度，分别用 xt 和 xt的导数表示。控制输入变量为u，也就是加速度，于是有如下形式:

所以这个系统的状态的方程为：
这里对应的的矩阵A大小为 2*2 ,矩阵B大小为 2*1。
貌似有了这个模型就能完全估计系统状态了，速度能计算出，位移也能计算出。那还要卡尔曼干嘛，问题是很多实际系统复杂到根本就建不了模。并且，即使你建立了较为准确的模型，只要你在某一步有误差，由递推公式，很可能不断将你的误差放大A倍（A就是那个状态转换矩阵），以至于最后得到的估计结果完全不能用了。回到最开始的那个笑话，如果那个完全凭预测的特种兵在某一步偏离了正确的路径，当他站在错误的路径上（而他自己以为是正确的）做下一步预测时，肯定走的路径也会错了，到最后越走越偏。
既然如此，我们就引进反馈。从概率论贝叶斯模型的观点来看前面预测的结果就是先验，测量出的结果就是后验。
测量值当然是由系统状态变量映射出来的，方程形式如下：
注意Z是测量值，大小为m*1(不是n*1，也不是1*1，后面将说明），H也是状态变量到测量的转换矩阵。大小为m*n。随机变量v是测量噪声。
同时对于匀加速模型，假设下车是匀加速远离我们，我们站在原点用超声波仪器测量小车离我们的距离。
也就是测量值直接等于位移。可能又会问，为什么不直接用测量值呢？测量值噪声太大了，根本不能直接用它来进行计算。试想一个本来是朝着一个方向做匀加速运动的小车，你测出来的位移确是前后移动（噪声影响），只根据测量的结果，你就以为车子一会往前开一会往后开。
对于状态方程中的系统噪声w和测量噪声v，假设服从如下多元高斯分布，并且w,v是相互独立的。其中Q,R为噪声变量的协方差矩阵。

看到这里自然要提个问题，为什么噪声模型就得服从高斯分布呢？请继续往下看。
对于小车匀加速运动的的模型，假设系统的噪声向量只存在速度分量上，，位移分量上的系统噪声为0。测量值只有位移，它的协方差矩阵大小是1*1，就是测量噪声的方差本身。
那么：
Q中，，位移上的为0，它们间协方差为0，即噪声间没有关联。

理论预测（先验）有了，测量值（后验）也有了，那怎么根据这两者得到最优的估计值呢？首先想到的就是加权，或者称之为反馈。
我们认定是预测（先验）值，是估计值，为测量值的预测，在下面的推导中，请注意估计和预测两者的区别，不混为一谈。由一般的反馈思想我们得到估计值：

其中，