文档介绍：卡尔曼滤波算法
卡尔曼滤波器是一个“最优化自回归数据处理算法”。对于解决大部分的问题，它是最优，效率最高甚至是最有用的。其广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等。近年来更被应用于卡尔曼滤波算法
卡尔曼滤波器是一个“最优化自回归数据处理算法”。对于解决大部分的问题，它是最优，效率最高甚至是最有用的。其广泛应用已经超过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(klk-1)是利用上一个状态预测的结果，X(k-1lk-1)是上一个状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0.
到现在为止，系统结果已经更新了，可是对应于X(klk-1)的协方差还没有更新。用P表示协方差：
P(klk-1)=AP(klk-1)A'+Q(4)
式子(4)中P(klk-1)是X(klk-1)对应的协方差，P(k-1lk-1)是X(k-1lk-1)对应的协方差，A表示A的转置矩阵，Q时系统过程的协方差。式(3),式(4)就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。
有了现在状态的预测结果，再收集现在状态的测量值。结合预测值和测量值，可以得到现在状态(k)的最优化估算值X(klk)：
X(klk)=X(klk-1)+Kg(k)(Z(k)-HX(klk-1))(5)
其中Kg为卡尔曼增益：
Kg(k)=P(klk-1)H'l(HP(klk-1)H'+R)(6)
到此为止，已经得到了k状态下最优的估算值X(klk)。但是，为了要令卡尔曼滤波器不断地运行下去，直到系统过程结束，还要更新k状态下X(klk)的covariance:
P(klk)=(I-Kg(k)H)P(klk-1)(7)
其中I为1的矩阵，对于单模型单测量，1=1。当系统进入式(k+1)状态时，P(klk)就是式(4)中的P(k-1lk-1)。这样，算法就可以自回归地运算下去。
卡尔曼滤波器的原理基本描述了，式(3)，(4)，(5)，(6)，(7)是5个基本公式。根据这5个公式，可以很容易地实现计算机程序。
仿真思路
把房间看