文档介绍：卡尔曼滤波算法
卡尔曼滤波器是一个“最优化自回归数据处理算法” 效率最高甚至是最有用的。其广泛应用已经超过据融合甚至在军事方面的雷达系统以及导弹追踪等。入，面部识别，图像分割，图像边缘检测等方面。
。对于解决大部分的问题，它是最优，
30 年，包括机器人导航，控制，传感器数近年来更被应用于计算机图像处理，列
卡尔曼滤波原理
首先要引入一个离散控制过程的系统，该系统可用一个线性随机微分方程来秒速：
X( k) =AX (k-1)+BU(k)+W(k) (1)
再加上系统的测量值：
Z(k)=HX (k)+V(k) (2)
上两式子中，X (k)是k时刻的系统状态，U (k)是k时刻对系统的控制量。 A和B
是系统参数，对于多模型系统，它们为矩阵。 Z ( k)是k时刻的测量值，H是测量系统的参数，对于多测量系统， H为矩阵。W( k)和V(k)分别表示过程和测量的噪声。它们被假
设成高斯白噪声，其协方差分别是 Q,R,这里假设它们不随系统状态变化而变化。
由于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声) ，卡尔曼滤波
器是最优的信息处理器。下面来估算系统的最优化输出。
首先利用系统的过程模型预测下个状态的系统。假设现在的系统状态是 k，根据系统的
模型，可以基于系统的上一状态而预测出现在状态：
X( k|k-1 ) =AX( k-1|k-1 ) +BU( k) ( 3)
式(3)中， X(k|k-1) 是利用上一个状态预测的结果， X (k-1|k-1 ) 是上一个状态最优的结果，U (k)为现在状态的控制量，如果没有控制量，它可以为 0.
到现在为止，系统结果已经更新了，可是对应于 X(k|k-1) 的协方差还没有更新。用 P 表示协方差：
P( k|k-1 ) =AP( k|k-1 ) A'+Q ( 4)
式子( 4)中 P( k|k-1 )是 X( k|k-1 ) 对应的协方差， P (k-1|k-1 ) 是 X( k-1|k-1 ) 对应的协方差，A'表示A的转置矩阵，Q时系统过程的协方差。式(3)，式(4)就是卡尔曼滤波器 5 个公式当中的前两个，也就是对系统的预测。
有了现在状态的预测结果，再收集现在状态的测量值。结合预测值和测量值，可以得到现在状态 (k) 的最优化估算值 X(k|k)：
X(k|k) =X (k|k-1) +Kg(k)( Z( k) -HX ( k|k-1 ) ) (5)
其中 Kg 为卡尔曼增益：
Kg(k)=P(k|k-1)H '|(HP(k|k-1)H '+R) ( 6)
到此为止，已经得到了 k 状态下最优的估算值 X ( k|k ) 。但是，为了要令卡尔曼滤波器不断地运行下去，直到系统过程结束，还要更新 k 状态下 X( k|k ) 的 covariance:
P( k|k ) =( I-Kg(k)H ) P(k|k-1) ( 7)
其中 I 为 1 的矩阵，对于单模型单测量， I=1 。当系统进入式 (k+1) 状态时， P(k|k) 就是式(4)中的P( k-1|k-1 )。这样，算法就可以自回归地运算下去。
卡尔曼滤波器的原理基本描述了，式( 3)，(4)，(5)，(6)，(7)是 5个基本公