OpenCV目标跟踪（五）-kalman滤波器

在实际的运动跟踪中，由于环境因素或者别的原因总会有一些噪声的存在，这样的跟踪效果就会变差，人们想到尽可能多的去利用测量结果来估计运动。这样，任务就可以分为两个阶段：第一阶段，即预测阶段，用过去得到的信息进一步修正以取得目标的下一个将会出现的位置。第二阶段，即校正阶段，我们获得一个测量，然后与基于前一次测量的预期值进行调整。
基于以上的任务，匈牙利数学家kalman提出了kalman滤波器的思想，这个滤波器背后的数学原理，有点复杂，这里给出了两个博客链接，他们都给出了对kalman滤波器数学原理的通俗解释。
http://www.cnblogs.com/feisky/archive/2009/11/09/1599247.html
http://blog.csdn.net/ilyhlf5201314/article/details/8255620

其中最为主要的是如下两个方程：
　

第一个方程是系统的运动方程，第二个方程是系统的观测方程，其中的参数A,B,H,都是矩阵，而OpenCV中自带的kalman示例程序的编程思路也是依据上面的两个方程。
具体的编程主体思路大致是这样的：
（1）初始化以下的变量：测量矩阵，转移矩阵，过程噪声协方差矩阵，测量噪声协方差矩阵，后验错误协方差矩阵等，而这些的矩阵的初始化，即是跟上述的两个方程中的A,B,H相关。
（2）调用OpenCV中的cvKalmanPredict（）函数，这个函数完成的正是任务一，预测阶段。
（3）调用OpenCV中的cvKalmanCorrect（）函数，这个函数完成的是任务二，校正阶段。
（4）重复步骤2和3，直至完成kalman滤波过程。

下面给出具体的kalman滤波示例，主题编程思路遵循上面所说的，但编程也有适当的技巧。示例程序如下：

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"

#include <stdio.h>

using namespace cv;

static inline Point calcPoint(Point2f center, double R, double angle)
{
    return center + Point2f((