粒子滤波C++程序.docx

粒子滤波C++程序.docx

《粒子滤波C++程序.docx》由会员分享，可在线阅读，更多相关《粒子滤波C++程序.docx（3页珍藏版）》请在冰点文库上搜索。

voidCFastTrackingDlg:

:

OnBnClickedButton4（）

{

//TODO:

Addyourcontrolnotificationhandlercodehere

//1.condensationsetup

constintstateNum=4;

constintmeasureNum=2;

constintsampleNum=2000;

CvConDensation*condens=cvCreateConDensation（stateNum,measureNum,sampleNum）;

CvMat*lowerBound;

CvMat*upperBound;

lowerBound=cvCreateMat（stateNum,1,CV_32F）;

upperBound=cvCreateMat（stateNum,1,CV_32F）;

cvmSet（lowerBound,0,0,0.0）;

cvmSet（upperBound,0,0,winWidth）;

cvmSet（lowerBound,1,0,0.0）;

cvmSet（upperBound,1,0,winHeight）;

cvmSet（lowerBound,2,0,0.0）;

cvmSet（upperBound,2,0,0.0）;

cvmSet（lowerBound,3,0,0.0）;

cvmSet（upperBound,3,0,0.0）;

floatA[stateNum][stateNum]={

1,0,1,0,

0,1,0,1,

0,0,1,0,

0,0,0,1

};

memcpy（condens->DynamMatr,A,sizeof（A））;

cvConDensInitSampleSet（condens,lowerBound,upperBound）;

CvRNGrng_state=cvRNG（0xffffffff）;

for（inti=0;i

condens->flSamples[i][0]=float（cvRandInt（&rng_state）%winWidth）;//width

condens->flSamples[i][1]=float（cvRandInt（&rng_state）%winHeight）;//height

}

CvFontfont;

cvInitFont（&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1）;

char*winName="condensation";

cvNamedWindow（winName）;

cvSetMouseCallback（winName,mouseEvent）;

IplImage*img=cvCreateImage（cvSize（winWidth,winHeight）,8,3）;

boolisPredictOnly=false;//triggerforpredictiononly,pressSPACEBAR

while

（1）{

//2.condensationprediction

CvPointpredict_pt=cvPoint（（int）condens->State[0],（int）condens->State[1]）;

floatvariance[measureNum]={0};

//getvariance/standarddeviationofeachstate

for（inti=0;i

{

//sum

floatsumState=0;

for（intj=0;jSamplesNum;j++）

{

sumState+=condens->flSamples[j][i];

}

//average

sumState/=sampleNum;

//variance

for（intj=0;jSamplesNum;j++）

{

variance[i]+=（condens->flSamples[j][i]-sumState）*

（condens->flSamples[j][i]-sumState）;

}

variance[i]/=sampleNum-1;

}

//3.updateparticalsconfidence

CvPointpt;

if（isPredictOnly）

{

pt=predict_pt;

}

else

{

pt=mousePosition;

}

for（inti=0;iSamplesNum;i++）

{

floatprobX=（float）exp（-1*（pt.x-condens->flSamples[i][0]）

*（pt.x-condens->flSamples[i][0]）/（2*variance[0]））;

floatprobY=（float）exp（-1*（pt.y-condens->flSamples[i][1]）

*（pt.y-condens->flSamples[i][1]）/（2*variance[1]））;

condens->flConfidence[i]=probX*probY;

}

//4.updatecondensation

cvConDensUpdateByTime（condens）;

//draw

cvSet（img,cvScalar（255,255,255,0））;

cvCircle（img,predict_pt,5,CV_RGB（0,255,0）,3）;//predictedpointwithgreen

charbuf[256];

sprintf_s（buf,256,"predictedposition:

（%3d,%3d）",predict_pt.x,predict_pt.y）;

cvPutText（img,buf,cvPoint（10,30）,&font,CV_RGB（0,0,0））;

if（!

isPredictOnly）

{

cvCircle（img,mousePosition,5,CV_RGB（255,0,0）,3）;//currentpositionwithred

sprintf_s（buf,256,"realposition:

（%3d,%3d）",mousePosition.x,mousePosition.y）;

cvPutText（img,buf,cvPoint（10,60）,&font,CV_RGB（0,0,0））;

}

cvShowImage（winName,img）;

intkey=cvWaitKey（30）;

if（key==27）{//esc

break;

}

elseif（key==''）

{//triggerforprediction

//isPredict=!

isPredict;

if（isPredictOnly）

{

isPredictOnly=false;

}

else

{

isPredictOnly=true;

}

}

}

cvReleaseImage（&img）;

cvReleaseConDensation（&condens）;

}