OpenCV 应用（1）卡尔曼滤波跟踪

0 卡尔曼OPENCV 预测鼠标位置

 

卡尔曼滤波不要求信号和噪声都是平稳过程的假设条件。对于每个时刻的系统扰动和观测误差（即噪声），只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误差为最小的真实信号的估计值。

因此，自从卡尔曼滤波理论问世以来，在通信系统、电力系统、航空航天、环境污染控制、工业控制、雷达信号处理等许多部门都得到了应用，取得了许多成功应用的成果。

卡尔曼滤波器会对含有噪声的输入数据流（比如计算机视觉中的视频输入）进行递归操作，并产生底层系统状态（比如视频中的位置）在统计意义上的最优估计。

 

卡尔曼滤波算法分为两个阶段： 

预测阶段：卡尔曼滤波器使用由当前点计算的协方差来估计目标的新位置； 
更新阶段：卡尔曼滤波器记录目标的位置，并为下一次循环计算修正协方差。

 

 

第一版

#include <cv.h>  
#include <cxcore.h>  
#include <highgui.h>  
  
#include <cmath>  
#include <vector>  
#include <iostream>  
using namespace std;  
  
const int winHeight=600;  
const int winWidth=800;  
  
  
CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1);  
  
//mouse event callback  
void mouseEvent(int event, int x, int y, int flags, void *param )  
{  
    if (event==CV_EVENT_MOUSEMOVE) {  
        mousePosition=cvPoint(x,y);  
    }  
}  
  
int main (void)  
{  
    //1.kalman filter setup  
    const int stateNum=4;  
    const int measureNum=2;  
    CvKalman* kalman = cvCreateKalman( stateNum, measureNum, 0 );//state(x,y,detaX,detaY)  
    CvMat* process_noise = cvCreateMat( stateNum, 1, CV_32FC1 );  
    CvMat* measurement = cvCreateMat( measureNum, 1, CV_32FC1 );//measurement(x,y)  
    CvRNG rng = cvRNG(-1);  
    float A[stateNum][stateNum] ={//transition matrix  
        1,0,1,0,  
        0,1,0,1,  
        0,0,1,0,  
        0,0,0,1  
    };  
  
    memcpy( kalman->transition_matrix->data.fl,A,sizeof(A));  
    cvSetIdentity(kalman->measurement_matrix,cvRealScalar(1) );  
    cvSetIdentity(kalman->process_noise_cov,cvRealScalar(1e-5));  
    cvSetIdentity(kalman->measurement_noise_cov,cvRealScalar(1e-1));  
    cvSetIdentity(kalman->error_cov_post,cvRealScalar(1));  
    //initialize post state of kalman filter at random  
    cvRandArr(&rng,kalman->state_post,CV_RAND_UNI,cvRealScalar(0),cvRealScalar(winHeight>winWidth?winWidth:winHeight));  
  
    CvFont font;  
    cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1);  
  
    cvNamedWindow("kalman");  
    cvSetMouseCallback("kalman",mouseEvent);  
    IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3);  
    while (1){  
        //2.kalman prediction  
        const CvMat* prediction=cvKalmanPredict(kalman,0);  
        CvPoint predict_pt=cvPoint((int)prediction->data.fl[0],(int)prediction->data.fl[1]);  
  
        //3.update measurement  
        measurement->data.fl[0]=(float)mousePosition.x;  
        measurement->data.fl[1]=(float)mousePosition.y;  
  
        //4.update  
        cvKalmanCorrect( kalman, measurement );       
  
        //draw   
        cvSet(img,cvScalar(255,255,255,0));  
        cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green  
        cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);//current position with red  
        char buf[256];  
        sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);  
        cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0));  
        sprintf_s(buf,256,"current positi