[opencv]Kalman滤波跟踪

1.Kalman滤波器原理

卡尔曼是匈牙利数学家，Kalman滤波器源于其博士毕业了论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。

该论文链接如下：

http://download.csdn.net/detail/kingcooper/9455107

斯坦利·施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时,发现他的方法对于解决阿波罗计划的轨道预测很有用,后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)发表。

卡尔曼滤波不要求信号和噪声都是平稳过程的假设条件。对于每个时刻的系统扰动和观测误差（即噪声），只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误差为最小的真实信号的估计值。因此，自从卡尔曼滤波理论问世以来，在通信系统、电力系统、航空航天、环境污染控制、工业控制、雷达信号处理等许多部门都得到了应用，取得了许多成功应用的成果。例如在图像处理方面，应用卡尔曼滤波对由于某些噪声影响而造成模糊的图像进行复原。在对噪声作了某些统计性质的假定后，就可以用卡尔曼的算法以递推的方式从模糊图像中得到均方差最小的真实图像，使模糊的图像得到复原。

这里我把Kalman相关公式列在前面，毕竟最终是要明确各个式子的含义和使用方法，而在实际例子说明的时候进行对照，理解起来更快一些。

首先对于离散控制过程的系统，其系统状态和系统测量值可进行以下表示：

X(k)： k时刻系统状态

A：     状态转移矩阵，对应opencv里kalman滤波器的transitionMatrix矩阵（关于opencv kalman滤波器详细定义会在2中给出）

B：     控制输入矩阵，对应opencv里kalman滤波器的controlMatrix矩阵

U(k)：k时刻对系统的控制量

Z(k)： k时刻的测量值

H：     系统测量矩阵，对应opencv里kalman滤波器的measurementMatrix矩阵

W(k)：系统过程噪声，为高斯白噪声，协方差为Q，对应opencv里的kalman滤波器的processNoiseCov矩阵

V(k)： 测量噪声，也为高斯白噪声，协方差为R，对应opencv里的kalman滤波器的measurementNoiseCov矩阵

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。

接下来便是核心的5个公式。

这5个式子可分成3部分看。

第一部分：

式(1)-(2)：预测值的计算

式(1)：计算基于k-1时刻状态对k时刻系统状态的预测值

X(k|k-1)：    基于k-1时刻状态对k时刻状态的预测值，对应opencv里kalman滤波器的predict()输出，即statePre矩阵

X(k-1|k-1)：k-1时刻状态的最优结果，对应opencv里kalman滤波器的上一次状态的statePost矩阵

U(k)：          k时刻的系统控制量，无则为0

A：               状态转移矩阵，对应opencv里kalman滤波器的transitionMatrix矩阵

B：               控制输入矩阵，对应opencv里kalman滤波器的controlMatrix矩阵

式(2)：计算X(k|k-1)对应的协方差的预测值

P(k|k-1)：   基于k-1时刻的协方差计算k时刻协方差的预测值，对应opencv里kalman滤波器的errorCovPre矩阵

P(k-1|k-1)：k-1时刻协方差的最优结果，对应opencv里kalman滤波器的上一次状态的errorCovPost矩阵

Q：           系统过程噪声协方差，对应opencv里kalman滤波器的processNoiseCov矩阵

第二部分：

式(3):增益的计算

Kg(k)：k时刻的kalman增益，为估计量的方差占总方差(估计量方差和测量方差)的比重，对应opencv里kalman滤波器的gain矩阵

H：     系统测量矩阵，对应opencv里kalman滤波器的measurementMatrix矩阵

R：     测量噪声协方差，对应opencv里的kalman滤波器的measurementNoiseCov矩阵

第三部分：

式(4)--(5):k时刻的更新

式(4)：计算k时刻系统状态最优值 

X(k|k)：k时刻系统状态的最优结果，对应opencv里kalman滤波器的k时刻状态的statePost矩阵

Z(k)：k时刻系统测量值

式(5)：计算k时刻系统最优结果对应的协方差

P(k|k)：k时刻系统最优结果对应的协方差，对应opencv里kalman滤波器的errorCovPost矩阵

以上便是Kalman的核心部分，运行(1)(2)-(3)-(4)(5)-(1)(2)......

输出即为X(k|k)，k时刻系统状态最优估计值。

运行流程见下图所示。

2.举例分析

比较常用的是关于温度的例子。

2.1房间温度计算例1

假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分布（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。

现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。

假如我们要估算k时刻的实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的 不确定度 是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。

由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的协方差(covariance)来判断。因为Kg=5^2/(5^2+4^2)，所以Kg=0.61，我们可以估算出k时刻的实际温度值是：23+0.61*(25-23)=24.22度。可以看出，因为温度计的协方差(covariance)比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。

现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.22度）的偏差。算法如下：((1-Kg)*5^2)^0.5=3.12。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的3.12就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。

就是这样，卡尔曼滤波器就不断的把(协方差(covariance)递归，从而估算出最优的温度值。

2.2房间温度计算例2

条件：

房间真实温度为24度

对温度的初始估计值为23.5度，误差的方差为1

温度计的测量值误差的标准差为0.5度

房间内连续两个时刻温度插值的标准差为0.02度

对应：Q=0.02的平方，R=0.5的平方，X(1|1)=23.5，P(1|1)=1，这里A为1，B为0，H为1

则式(1)和式(2)便可进行计算，由此得到(3)，进而对(4)和(5)进行计算更新。

不断更新以估算最优温度值。

3.opencv Kalman函数

链接：

http://www.opencv.org.cn/opencvdoc/2.3.2/html/modules/video/doc/motion_analysis_and_object_tracking.html?highlight=kalman#KalmanFilter

KalmanFilter:: KalmanFilter ( int  dynamParams , int  measureParams , int  controlParams =0, int  type =CV_32F )

Parameters:

• dynamParams – Dimensionality of the state.
• measureParams – Dimensionality of the measurement.
• controlParams – Dimensionality of the control vector.
• type – Type of the created matrices that should be CV_32F or CV_64F.

dynamParams：状态的维数

measureParams ：测量的维数

controlParams ：控制量的维数

type：创建矩阵类型（CV_32F or CV_64F）

opencv内KalmanFilter的定义如下：

/*!
 Kalman filter.

 The class implements standard Kalman filter \url{http://en.wikipedia.org/wiki/Kalman_filter}.
 However, you can modify KalmanFilter::transitionMatrix, KalmanFilter::controlMatrix and
 KalmanFilter::measurementMatrix to get the extended Kalman filter functionality.
*/
class CV_EXPORTS_W KalmanFilter
{
public:
    //! the default constructor
    CV_WRAP KalmanFilter();
    //! the full constructor taking the dimensionality of the state, of the measurement and of the control vector
    CV_WRAP KalmanFilter(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);
    //! re-initializes Kalman filter. The previous content is destroyed.
    void init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);

    //! computes predicted state
    CV_WRAP const Mat& predict(const Mat& control=Mat());
    //! updates the predicted state from the measurement
    CV_WRAP const Mat& correct(const Mat& measurement);

    Mat statePre;           //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
    Mat statePost;          //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
    Mat transitionMatrix;   //!< state transition matrix (A)
    Mat controlMatrix;      //!< control matrix (B) (not used if there is no control)
    Mat measurementMatrix;  //!< measurement matrix (H)
    Mat processNoiseCov;    //!< process noise covariance matrix (Q)
    Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
    Mat errorCovPre;        //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
    Mat gain;               //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
    Mat errorCovPost;       //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)

    // temporary matrices
    Mat temp1;
    Mat temp2;
    Mat temp3;
    Mat temp4;
    Mat temp5;
};

4.opencv实现对鼠标的kalman跟踪

初始化：

int dynamParams = 4;//状态数，这里鼠标坐标X,Y和速度DX,DY
int measureParams = 2;//观测量数量，这里只写了X,Y

kalman滤波器：

KalmanFilter KF(dynamParams,measureParams,0);//第三个为控制量参数，这里没有

转移矩阵A：

KF.transitionMatrix = *(Mat_<float>(4, 4) <<
		1, 0, 1, 0,
		0, 1, 0, 1,
		0, 0, 1, 0,
		0 ,0, 0, 1);

定义各个矩阵：

        setIdentity(KF.measurementMatrix);//测量矩阵H
	setIdentity(KF.processNoiseCov,Scalar::all(1e-5));//过程噪声协方差矩阵，标准差为1e-5
	setIdentity(KF.measurementNoiseCov,Scalar::all(1e-1));//测量噪声协方差矩阵R，标准差为1e-1
	setIdentity(KF.errorCovPost,Scalar::all(1));//P(1|1),估计协方差矩阵
        Mat state(dynamParams, 1, CV_32F);
	Mat processNoise(dynamParams, 1, CV_32F);
	Mat measurement = Mat::zeros(measureParams, 1, CV_32F);
        randn(KF.statePost,Scalar::all(0),Scalar::all(0.1));//初始状态值X(1|1)
        randn(state,Scalar::all(0),Scalar::all(0.1));

鼠标响应函数相关操作见：

http://blog.csdn.net/kingcooper/article/details/50788269

每次计算部分：

	Mat img(480, 480, CV_8UC3, Scalar(255, 255, 255));
	Mat prediction = KF.predict();
	Point predictPt = Point(prediction.at<float>(0),prediction.at<float>(1));
	measurement.at<float>(0) = (float)mousepoint.x;
	measurement.at<float>(1) = (float)mousepoint.y;
	KF.correct(measurement);
	Point statePt = Point(KF.statePost.at<float>(0),KF.statePost.at<float>(1));

得到GIF效果如下：

currentpoint为setMouseCallback获取鼠标值

predictpoint为基于上一次最优结果计算的预测坐标值，即X(k|k-1)

Kalmanpoint为现在最优结果坐标，即X(k|k)

参考链接：

https://en.wikipedia.org/wiki/Kalman_filter

http://baike.baidu.com/link?url=Dkhc_U50LM8nGysNYB1cbMQsaFiiz-HCas6DNP9yfo1xMIEj2caU5zpVkoNcf_4J033_r10xqrPKrMgl_-X_tq

http://blog.csdn.net/xiahouzuoxin/article/details/39582483

http://www.cnblogs.com/tornadomeet/archive/2012/08/19/2646412.html

http://blog.csdn.net/yang_xian521/article/details/7050398

http://blog.csdn.net/carson2005/article/details/7367135

http://blog.csdn.net/onezeros/article/details/6318944