卡尔曼滤波以及opencv源代码分析

卡尔曼滤波

参考链接：
http://www.cnblogs.com/jcchen1987/p/4371439.html
http://www.cnblogs.com/rubbninja/p/6220284.html
Kalman滤波是一种线性滤波与预测方法，原文为：A New Approach to Linear Filtering and Prediction Problems。文章推导很复杂，看了一半就看不下去了，既然不能透彻理解其原理，但总可以通过实验来理解其具体的使用方法。

　　Kalman滤波分为2个步骤，预测(predict)和校正(correct)。预测是基于上一时刻状态估计当前时刻状态，而校正则是综合当前时刻的估计状态与观测状态，估计出最优的状态。预测与校正的过程如下：
1) 预测：

2) 校正

• 
  
• 公式（步骤）1：由上一时刻的状态预测当前状态，加上外界的输入。
• 公式（步骤）2：预测过程增加了新的不确定性Q，加上之前存在的不确定性。
• 公式（步骤）3：由预测结果的不确定性Pk和观测结果的不确定性RR计算卡尔曼增益（权重）。
• 公式（步骤）4：对预测结果和观测结果做加权平均，得到当前时刻的状态估计。
• 公式（步骤）5：更新Pk，代表本次状态估计的不确定性。
  

xk : k时刻的状态
A : 状态转移矩阵，和具体的线性系统相关
uk : K时刻外界对系统的作用
B : 输入控制矩阵，外界的影响如何转化为对状态的影响
P : 误差矩阵
Q : 预测噪声协方差