运动跟踪（五）：Kalman滤波

class CV_EXPORTS_W KalmanFilter
{
public:
    //! the default constructor
    CV_WRAP KalmanFilter();
    //! the full constructor taking the dimensionality of the state, of the measurement and of the control vector
    CV_WRAP KalmanFilter(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);
    //! re-initializes Kalman filter. The previous content is destroyed.
    void init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);

    //! computes predicted state
    CV_WRAP const Mat& predict(const Mat& control=Mat());
    //! updates the predicted state from the measurement
    CV_WRAP const Mat& correct(const Mat& measurement);

    Mat statePre;           //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
    Mat statePost;          //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
    Mat transitionMatrix;   //!< state transition matrix (A)
    Mat controlMatrix;      //!< control matrix (B) (not used if there is no control)
    Mat measurementMatrix;  //!< measurement matrix (H)
    Mat processNoiseCov;    //!< process noise covariance matrix (Q)
    Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
    Mat errorCovPre;        //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
    Mat gain;               //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
    Mat errorCovPost;       //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)

    // temporary matrices
    Mat temp1;
    Mat temp2;
    Mat temp3;
    Mat temp4;
    Mat temp5;
};

（1）KalmanFilter示例

// KalmanFilter.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"

#include <stdio.h>

using namespace cv;

static inline Point calcPoint(Point2f center, double R, double angle)
{
	return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R;
}

static void help()
{
	printf( "\nExamle of c calls to OpenCV's Kalman filter.\n"
		"   Tracking of rotating point.\n"
		"   Rotation speed is constant.\n"
		"   Both state and measurements vectors are 1D (a point angle),\n"
		"   Measurement is the real point angle + gaussian noise.\n"
		"   The real and the estimated points are connected with yellow line segment,\n"
		"   the real and the measured points are connected with red line segment.\n"
		"   (if Kalman filter works correctly,\n"
		"    the yellow segment should be shorter than the red one).\n"
		"\n"
		"   Pressing any key (except ESC) will reset the tracking with a different speed.\n"
		"   Pressing ESC will stop the program.\n"
		);
}

int main(int, char**)
{
	help();
	Mat img(500, 500, CV_8UC3);
	KalmanFilter KF(2, 1, 0);
	Mat state(2, 1, CV_32F); /* (phi, delta_phi) */
	Mat processNoise(2, 1, CV_32F);
	Mat measurement = Mat::zeros(1, 1, CV_32F);
	char code = (char)-1;

	for(;;)
	{
		randn( state, Scalar::all(0), Scalar::all(0.1) );
		KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 1, 0, 1);

		setIdentity(KF.measurementMatrix);
		setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
		setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
		setIdentity(KF.errorCovPost, Scalar::all(1));

		randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));

		for(;;)
		{
			Point2f center(img.cols*0.5f, img.rows*0.5f);
			float R = img.cols/3.f;
			double stateAngle = state.at<float>(0);
			Point statePt = calcPoint(center, R, stateAngle);

			Mat prediction = KF.predict();
			double predictAngle = prediction.at<float>(0);
			Point predictPt = calcPoint(center, R, predictAngle);

			randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0)));

			// generate measurement
			measurement += KF.measurementMatrix*state;

			double measAngle = measurement.at<float>(0);
			Point measPt = calcPoint(center, R, measAngle);

			// plot points
#define drawCross( center, color, d )                                 \
	line( img, Point( center.x - d, center.y - d ),                \
	Point( center.x + d, center.y + d ), color, 1, CV_AA, 0); \
	line( img, Point( center.x + d, center.y - d ),                \
	Point( center.x - d, center.y + d ), color, 1, CV_AA, 0 )

			img = Scalar::all(0);
			drawCross( statePt, Scalar(255,255,255), 3 );
			drawCross( measPt, Scalar(0,0,255), 3 );
			drawCross( predictPt, Scalar(0,255,0), 3 );
			line( img, statePt, measPt, Scalar(0,0,255), 3, CV_AA, 0 );
			line( img, statePt, predictPt, Scalar(0,255,255), 3, CV_AA, 0 );

			if(theRNG().uniform(0,4) != 0)
				KF.correct(measurement);

			randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));
			state = KF.transitionMatrix*state + processNoise;

			imshow( "Kalman", img );
			code = (char)waitKey(100);

			if( code > 0 )
				break;
		}
		if( code == 27 || code == 'q' || code == 'Q' )
			break;
	}

	return 0;
}

（2）测试效果

下面的内容是转载：

原文：https://blog.csdn.net/onezeros/article/details/6318944

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

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，依次类推
所以这个矩阵还是很容易却确定的，可以根据自己的实际情况定制转移矩阵

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

[cpp] view plain copy

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

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

 

demo snapshot：