kalman 滤波演示与opencv代码

柔光的暖阳◎ 2022-08-27 03:39 152阅读 0赞

在机器视觉中追踪时常会用到预测算法，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 plain][] [copy][view plain]

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.   \};

看下图就清楚了

![2895444_1302615138MmZf.jpg][]

X1=X+dx，依次类推  
所以这个矩阵还是很容易却确定的，可以根据自己的实际情况定制转移矩阵

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

**\[cpp\]** [view plain][] [copy][view plain]

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，下面的例子或许更有用些。

**\[cpp\]** [view plain][] [copy][view plain]

1.  \#include <cv.h>
2.  \#include <cxcore.h>
3.  \#include <highgui.h>
4.  
5.  \#include <cmath>
6.  \#include <vector>
7.  \#include <iostream>
8.  **using****namespace** std; 
9.  
10. **const****int** winHeight=600; 
11. **const****int** winWidth=800; 
12. 
13. 
14. CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1); 
15. 
16. //mouse event callback
17. **void** mouseEvent(**int** event, **int** x, **int** y, **int** flags, **void** \*param ) 
18. \{ 
19. **if** (event==CV\_EVENT\_MOUSEMOVE) \{ 
20.  mousePosition=cvPoint(x,y); 
21.  \} 
22. \} 
23. 
24. **int** main (**void**) 
25. \{ 
26. //1.kalman filter setup
27. **const****int** stateNum=4; 
28. **const****int** measureNum=2; 
29.  CvKalman\* kalman = cvCreateKalman( stateNum, measureNum, 0 );//state(x,y,detaX,detaY)
30.  CvMat\* process\_noise = cvCreateMat( stateNum, 1, CV\_32FC1 ); 
31.  CvMat\* measurement = cvCreateMat( measureNum, 1, CV\_32FC1 );//measurement(x,y)
32.  CvRNG rng = cvRNG(-1); 
33. **float** A\[stateNum\]\[stateNum\] =\{ //transition matrix
34.  1,0,1,0, 
35.  0,1,0,1, 
36.  0,0,1,0, 
37.  0,0,0,1 
38.  \}; 
39. 
40.  memcpy( kalman->transition\_matrix->data.fl,A,**sizeof**(A)); 
41.  cvSetIdentity(kalman->measurement\_matrix,cvRealScalar(1) ); 
42.  cvSetIdentity(kalman->process\_noise\_cov,cvRealScalar(1e-5)); 
43.  cvSetIdentity(kalman->measurement\_noise\_cov,cvRealScalar(1e-1)); 
44.  cvSetIdentity(kalman->error\_cov\_post,cvRealScalar(1)); 
45. //initialize post state of kalman filter at random
46.  cvRandArr(&rng,kalman->state\_post,CV\_RAND\_UNI,cvRealScalar(0),cvRealScalar(winHeight>winWidth?winWidth:winHeight)); 
47. 
48.  CvFont font; 
49.  cvInitFont(&font,CV\_FONT\_HERSHEY\_SCRIPT\_COMPLEX,1,1); 
50. 
51.  cvNamedWindow("kalman"); 
52.  cvSetMouseCallback("kalman",mouseEvent); 
53.  IplImage\* img=cvCreateImage(cvSize(winWidth,winHeight),8,3); 
54. **while** (1)\{ 
55. //2.kalman prediction
56. **const** CvMat\* prediction=cvKalmanPredict(kalman,0); 
57.  CvPoint predict\_pt=cvPoint((**int**)prediction->data.fl\[0\],(**int**)prediction->data.fl\[1\]); 
58. 
59. //3.update measurement
60.  measurement->data.fl\[0\]=(**float**)mousePosition.x; 
61.  measurement->data.fl\[1\]=(**float**)mousePosition.y; 
62. 
63. //4.update
64.  cvKalmanCorrect( kalman, measurement ); 
65. 
66. //draw 
67.  cvSet(img,cvScalar(255,255,255,0)); 
68.  cvCircle(img,predict\_pt,5,CV\_RGB(0,255,0),3);//predicted point with green
69.  cvCircle(img,mousePosition,5,CV\_RGB(255,0,0),3);//current position with red
70. **char** buf\[256\]; 
71.  sprintf\_s(buf,256,"predicted position:(%3d,%3d)",predict\_pt.x,predict\_pt.y); 
72.  cvPutText(img,buf,cvPoint(10,30),&font,CV\_RGB(0,0,0)); 
73.  sprintf\_s(buf,256,"current position :(%3d,%3d)",mousePosition.x,mousePosition.y); 
74.  cvPutText(img,buf,cvPoint(10,60),&font,CV\_RGB(0,0,0)); 
75. 
76.  cvShowImage("kalman", img); 
77. **int** key=cvWaitKey(3); 
78. **if** (key==27)\{ //esc 
79. **break**; 
80.  \} 
81.  \} 
82. 
83.  cvReleaseImage(&img); 
84.  cvReleaseKalman(&kalman); 
85. **return** 0; 
86. \}

kalman filter 视频演示：

[http://v.youku.com/v\_show/id\_XMjU4MzEyODky.html][http_v.youku.com_v_show_id_XMjU4MzEyODky.html]

demo snapshot：

![2895444_1302616101TU55.jpg][]

[view plain]: http://blog.csdn.net/onezeros/article/details/6318944#
[2895444_1302615138MmZf.jpg]: /images/20220824/e907945dc83b4498a0b45a867ab5987d.png
[http_v.youku.com_v_show_id_XMjU4MzEyODky.html]: http://v.youku.com/v_show/id_XMjU4MzEyODky.html
[2895444_1302616101TU55.jpg]: /images/20220824/6c9e041010d241a8a0c7b8557df2556a.png