卡尔曼滤波经典例子（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) 转移矩阵或者说增益矩阵的值好像有点莫名其妙 view plaincopyprint?[*]float A ={//transition matrix [*] 1,0,1,0,[*] 0,1,0,1,[*] 0,0,1,0,[*] 0,0,0,1[*] }; 看下图就清楚了 http://hi.csdn.net/attachment/201104/12/2895444_1302615138MmZf.jpg X1=X+dx，依次类推所以这个矩阵还是很容易却确定的，可以根据自己的实际情况定制转移矩阵 同样的方法，三维坐标的转移矩阵可以如下 view plaincopyprint?[*]float A ={//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，下面的例子或许更有用些。 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 ={//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,(int)prediction->data.fl);[*][*] //3.update measurement [*] measurement->data.fl=(float)mousePosition.x;[*] measurement->data.fl=(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;[*] 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.htmldemo snapshot： http://hi.csdn.net/attachment/201104/12/2895444_1302616101TU55.jpg 东西不错，很受用！ 谢谢楼主分享 学习了
页:
[1]