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)

转移矩阵或者说增益矩阵的值好像有点莫名其妙

1. float A[stateNum][stateNum] ={//transition matrix
2. 1,0,1,0,
3. 0,1,0,1,
4. 0,0,1,0,
5. 0,0,0,1
6. };

看下图就清楚了

X1=X+dx，依次类推

所以这个矩阵还是很容易却确定的，可以根据自己的实际情况定制转移矩阵

同样的方法，三维坐标的转移矩阵可以如下

1. float A[stateNum][stateNum] ={//transition matrix
2. 1,0,0,1,0,0,
3. 0,1,0,0,1,0,
4. 0,0,1,0,0,1,
5. 0,0,0,1,0,0,
6. 0,0,0,0,1,0,
7. 0,0,0,0,0,1
8. };

当然并不一定得是1和0

2.预测cvKalmanPredict，然后读出自己需要的值

3.更新观测矩阵 

4.更新CvKalman

只有第一步麻烦些。上述这几步跟代码中的序号对应

如果你在做tracking，下面的例子或许更有用些。

1. #include <cv.h>
2. #include <cxcore.h>
3. #include <highgui.h>
4. #include <cmath>
5. #include <vector>
6. #include <iostream>
7. using namespace std;
8. const int winHeight=600;
9. const int winWidth=800;
10. CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1);
11. //mouse event callback
12. void mouseEvent(int event, int x, int y, int flags, void *param )
13. {
14. if (event==CV_EVENT_MOUSEMOVE) {
15. mousePosition=cvPoint(x,y);
16. }
17. }
18. int main (void)
19. {
20. //1.kalman filter setup
21. const int stateNum=4;
22. const int measureNum=2;
23. CvKalman* kalman = cvCreateKalman( stateNum, measureNum, 0 );//state(x,y,detaX,detaY)
24. CvMat* process_noise = cvCreateMat( stateNum, 1, CV_32FC1 );
25. CvMat* measurement = cvCreateMat( measureNum, 1, CV_32FC1 );//measurement(x,y)
26. CvRNG rng = cvRNG(-1);
27. float A[stateNum][stateNum] ={//transition matrix
28. 1,0,1,0,
29. 0,1,0,1,
30. 0,0,1,0,
31. 0,0,0,1
32. };
33. memcpy( kalman->transition_matrix->data.fl,A,sizeof(A));
34. cvSetIdentity(kalman->measurement_matrix,cvRealScalar(1) );
35. cvSetIdentity(kalman->process_noise_cov,cvRealScalar(1e-5));
36. cvSetIdentity(kalman->measurement_noise_cov,cvRealScalar(1e-1));
37. cvSetIdentity(kalman->error_cov_post,cvRealScalar(1));
38. //initialize post state of kalman filter at random
39. cvRandArr(&rng,kalman->state_post,CV_RAND_UNI,cvRealScalar(0),cvRealScalar(winHeight>winWidth?winWidth:winHeight));
40. CvFont font;
41. cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1);
42. cvNamedWindow("kalman");
43. cvSetMouseCallback("kalman",mouseEvent);
44. IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3);
45. while (1){
46. //2.kalman prediction
47. const CvMat* prediction=cvKalmanPredict(kalman,0);
48. CvPoint predict_pt=cvPoint((int)prediction->data.fl[0],(int)prediction->data.fl[1]);
49. //3.update measurement
50. measurement->data.fl[0]=(float)mousePosition.x;
51. measurement->data.fl[1]=(float)mousePosition.y;
52. //4.update
53. cvKalmanCorrect( kalman, measurement );
54. //draw
55. cvSet(img,cvScalar(255,255,255,0));
56. cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green
57. cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);//current position with red
58. char buf[256];
59. sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);
60. cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0));
61. sprintf_s(buf,256,"current position :(%3d,%3d)",mousePosition.x,mousePosition.y);
62. cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0));
63. cvShowImage("kalman", img);
64. int key=cvWaitKey(3);
65. if (key==27){//esc
66. break;
67. }
68. }
69. cvReleaseImage(&img);
70. cvReleaseKalman(&kalman);
71. return 0;
72. }

kalman filter 视频演示：

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

demo snapshot：