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