粒子滤波 演示与opencv代码

粒子滤波的理论实在是太美妙了，用一组不同权重的随机状态来逼近复杂的概率密度函数。其再非线性、非高斯系统中具有优良的特性。opencv给出了一个实现，但是没有给出范例，学习过程中发现网络上也找不到。learning opencv一书中有介绍，但距离直接使用还是有些距离。在经过一番坎坷后，终于可以用了，希望对你有帮助。

 

 本文中给出的例子跟 我的另一篇博文是同一个应用例子，都是对二维坐标进行平滑、预测

使用方法：

1.创建并初始化

const int stateNum=4;//状态数
 const int measureNum=2;//测量变量数
 const int sampleNum=2000;//粒子数

 CvConDensation* condens = cvCreateConDensation(stateNum,measureNum,sampleNum);

在不影响性能的情况下，粒子数量越大，系统表现的越稳定

其他初始化内容请参考learning opencv

2.预测
3.更新例子可信度，也就是权重。本例中更新方法与learning opencv中有所不同，想看代码 
4.更新CvConDensation

 

代码：

[cpp] view plaincopy

1. #include <cv.h>  
2. #include <cxcore.h>  
3. #include <highgui.h>  
4. #include <cvaux.h>  
5.   
6. #include <cmath>  
7. #include <vector>  
8. #include <iostream>  
9. using namespace std;  
10.   
11. const int winHeight=600;  
12. const int winWidth=800;  
13.   
14.   
15. CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1);  
16.   
17. //mouse event callback  
18. void mouseEvent(int event,int x,int y,int flags,void *param )  
19. {  
20.     if (event==CV_EVENT_MOUSEMOVE) {  
21.         mousePosition=cvPoint(x,y);  
22.     }  
23. }  
24.   
25. int main (void)  
26. {  
27.     //1.condensation setup  
28.     const int stateNum=4;  
29.     const int measureNum=2;  
30.     const int sampleNum=2000;  
31.   
32.     CvConDensation* condens = cvCreateConDensation(stateNum,measureNum,sampleNum);  
33.     CvMat* lowerBound;  
34.     CvMat* upperBound;  
35.     lowerBound = cvCreateMat(stateNum, 1, CV_32F);  
36.     upperBound = cvCreateMat(stateNum, 1, CV_32F);  
37.     cvmSet(lowerBound,0,0,0.0 );   
38.     cvmSet(upperBound,0,0,winWidth );  
39.     cvmSet(lowerBound,1,0,0.0 );   
40.     cvmSet(upperBound,1,0,winHeight );  
41.     cvmSet(lowerBound,2,0,0.0 );   
42.     cvmSet(upperBound,2,0,0.0 );  
43.     cvmSet(lowerBound,3,0,0.0 );   
44.     cvmSet(upperBound,3,0,0.0 );  
45.     float A[stateNum][stateNum] ={  
46.         1,0,1,0,  
47.         0,1,0,1,  
48.         0,0,1,0,  
49.         0,0,0,1  
50.     };  
51.     memcpy(condens->DynamMatr,A,sizeof(A));  
52.     cvConDensInitSampleSet(condens, lowerBound, upperBound);  
53.   
54.     CvRNG rng_state = cvRNG(0xffffffff);  
55.     for(int i=0; i < sampleNum; i++){  
56.         condens->flSamples[i][0] = float(cvRandInt( &rng_state ) % winWidth); //width  
57.         condens->flSamples[i][1] = float(cvRandInt( &rng_state ) % winHeight);//height  
58.     }  
59.   
60.     CvFont font;  
61.     cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1);  
62.   
63.     char* winName="condensation";  
64.     cvNamedWindow(winName);  
65.     cvSetMouseCallback(winName,mouseEvent);  
66.     IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3);  
67.     bool isPredictOnly=false;//trigger for prediction only,press SPACEBAR  
68.     while (1){  
69.         //2.condensation prediction  
70.         CvPoint predict_pt=cvPoint((int)condens->State[0],(int)condens->State[1]);  
71.   
72.         float variance[measureNum]={0};       
73.         //get variance/standard deviation of each state  
74.         for (int i=0;i<measureNum;i++) {  
75.             //sum  
76.             float sumState=0;  
77.             for (int j=0;j<condens->SamplesNum;j++) {  
78.                 sumState+=condens->flSamples[i][j];  
79.             }  
80.             //average  
81.             sumState/=sampleNum;  
82.             //variance  
83.             for (int j=0;j<condens->SamplesNum;j++) {  
84.                 variance[i]+=(condens->flSamples[i][j]-sumState)*  
85.                     (condens->flSamples[i][j]-sumState);  
86.             }  
87.             variance[i]/=sampleNum-1;  
88.         }  
89.         //3.update particals confidence  
90.         CvPoint pt;  
91.         if (isPredictOnly) {  
92.             pt=predict_pt;  
93.         }else{  
94.             pt=mousePosition;  
95.         }  
96.         for (int i=0;i<condens->SamplesNum;i++) {  
97.             float probX=(float)exp(-1*(pt.x-condens->flSamples[i][0])  
98.                 *(pt.x-condens->flSamples[i][0])/(2*variance[0]));  
99.             float probY=(float)exp(-1*(pt.y-condens->flSamples[i][1])  
100.                 *(pt.y-condens->flSamples[i][1])/(2*variance[1]));  
101.             condens->flConfidence[i]=probX*probY;  
102.         }  
103.         //4.update condensation  
104.         cvConDensUpdateByTime(condens);  
105.           
106.         //draw   
107.         cvSet(img,cvScalar(255,255,255,0));  
108.         cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green  
109.         char buf[256];  
110.         sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);  
111.         cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0));  
112.         if (!isPredictOnly) {  
113.             cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);//current position with red  
114.             sprintf_s(buf,256,"real position :(%3d,%3d)",mousePosition.x,mousePosition.y);  
115.             cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0));  
116.         }  
117.           
118.         cvShowImage(winName, img);  
119.         int key=cvWaitKey(30);  
120.         if (key==27){//esc     
121.             break;  
122.         }else if (key==' ') {//trigger for prediction  
123.             //isPredict=!isPredict;  
124.             if (isPredictOnly) {  
125.                 isPredictOnly=false;  
126.             }else{  
127.                 isPredictOnly=true;  
128.             }  
129.         }  
130.     }        
131.   
132.     cvReleaseImage(&img);  
133.     cvReleaseConDensation(&condens);  
134.     return 0;  
135. }  

 

kalman filter 视频演示：

演示中粒子数分别为100,200,2000

请仔细观测效果

http://v.youku.com/v_show/id_XMjU4MzE0ODgw.html

demo snapshot：