卡尔曼滤波经典例子（opencv）

kalman 滤波 演示与opencv代码     在机器视觉中追踪时常会用到预测算法，kalman是你一定知道的。它可以用来预测各种状态，比如说位置，速度等。关于它的理论有很多很好的文献可以参考。opencv给出了kalman filter的一个实现，而且有范例，但估计不少人对它的使用并不清楚，因为我也是其中一个。本文的应用是对二维坐标进行预测和平滑  使用方法： 1、初始化 const int stateNum=4;//状态数，包括（x，y，dx，dy）坐标及速度（每次移动的距离）const int measureNum=2;//观测量，能看到的是坐标值，当然也可以自己计算速度，但没必要Kalman* kalman = cvCreateKalman( stateNum, measureNum, 0 );//state(x,y,detaX,detaY) 转移矩阵或者说增益矩阵的值好像有点莫名其妙   [cpp] view plaincopyprint?  

• float A[stateNum][stateNum] ={//transition matrix   
•          1,0,1,0,  
•          0,1,0,1,  
•          0,0,1,0,  
•          0,0,0,1  
•      };  

   看下图就清楚了

                               
登录/注册后可看大图

X1=X+dx，依次类推所以这个矩阵还是很容易却确定的，可以根据自己的实际情况定制转移矩阵 同样的方法，三维坐标的转移矩阵可以如下   [cpp] view plaincopyprint?  

• float A[stateNum][stateNum] ={//transition matrix   
•          1,0,0,1,0,0,  
•          0,1,0,0,1,0,  
•          0,0,1,0,0,1,  
•          0,0,0,1,0,0,  
•          0,0,0,0,1,0,  
•          0,0,0,0,0,1  
•      };  

   当然并不一定得是1和0 2.预测cvKalmanPredict，然后读出自己需要的值3.更新观测矩阵4.更新CvKalman  只有第一步麻烦些。上述这几步跟代码中的序号对应  如果你在做tracking，下面的例子或许更有用些。    [cpp] view plaincopyprint?  

• #include <cv.h>   
• #include <cxcore.h>   
• #include <highgui.h>   
•   
• #include <cmath>   
• #include <vector>   
• #include <iostream>   
• usingnamespace std;  
•    
• constint winHeight=600;  
• constint 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   
•      constint stateNum=4;  
•      constint 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=,=)",predict_pt.x,predict_pt.y);  
•          cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0));  
•          sprintf_s(buf,256,"current position =,=)",mousePosition.x,mousePosition.y);  
•          cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0));  
•            
•          cvShowImage("kalman", img);  
•          int key=cvWaitKey(3);  
•          if (key==27){//esc      
•              break;     
•          }  
•      }        
•   
•      cvReleaseImage(&img);  
•      cvReleaseKalman(&kalman);  
•      return 0;  
• }  

   kalman filter 视频演示： http://v.youku.com/v_show/id_XMjU4MzEyODky.html  demo snapshot：

                               
登录/注册后可看大图

东西不错，很受用！

谢谢楼主分享

学习了