卡尔曼滤波算法示例代码

#include <cv.h>
#include <cxcore.h>
#include <highgui.h>

#include <cmath>
#include <vector>
#include <iostream>
using namespace std;

static const int winHeight = 600;
static const int winWidth = 800;


static CvPoint mousePosition = cvPoint(winWidth >> 1, winHeight >> 1);

//mouse event callback
static void mouseEvent(int event, int x, int y, int flags, void *param)
{
	if (event == CV_EVENT_MOUSEMOVE) {
		mousePosition = cvPoint(x, y);
	}
}

int main(void)
{
	//1.kalman filter setup
	const int stateNum = 4;
	const int measureNum = 2;
	CvKalman* kalman = cvCreateKalman(stateNum, measureNum, 0);//state(x,y,detaX,detaY)
	CvMat* process_noise = cvCreateMat(stateNum, 1, CV_32FC1);
	CvMat* measurement = cvCreateMat(measureNum, 1, CV_32FC1);//measurement(x,y)
	CvRNG rng = cvRNG(-1);
	float A[stateNum][stateNum] = {//transition matrix
		1, 0, 1, 0,
		0, 1, 0, 1,
		0, 0, 1, 0,
		0, 0, 0, 1
	};

	memcpy(kalman->transition_matrix->data.fl, A, sizeof(A));
	cvSetIdentity(kalman->measurement_matrix, cvRealScalar(1));
	cvSetIdentity(kalman->process_noise_cov, cvRealScalar(1e-5));
	cvSetIdentity(kalman->measurement_noise_cov, cvRealScalar(1e-1));
	cvSetIdentity(kalman->error_cov_post, cvRealScalar(1));
	//initialize post state of kalman filter at random
	cvRandArr(&rng, kalman->state_post, CV_RAND_UNI, cvRealScalar(0), cvRealScalar(winHeight>winWidth ? winWidth : winHeight));

	CvFont font;
	cvInitFont(&font, CV_FONT_HERSHEY_SCRIPT_COMPLEX, 1, 1);

	cvNamedWindow("kalman");
	cvSetMouseCallback("kalman", mouseEvent);
	IplImage* img = cvCreateImage(cvSize(winWidth, winHeight), 8, 3);
	while (1){
		//2.kalman prediction
		const CvMat* prediction = cvKalmanPredict(kalman, 0);
		CvPoint predict_pt = cvPoint((int)prediction->data.fl[0], (int)prediction->data.fl[1]);

		//3.update measurement
		measurement->data.fl[0] = (float)mousePosition.x;
		measurement->data.fl[1] = (float)mousePosition.y;

		//4.update
		cvKalmanCorrect(kalman, measurement);

		//draw 
		cvSet(img, cvScalar(255, 255, 255, 0));
		cvCircle(img, predict_pt, 5, CV_RGB(0, 255, 0), 3);//predicted point with green
		cvCircle(img, mousePosition, 5, CV_RGB(255, 0, 0), 3);//current position with red
		char buf[256];
		sprintf_s(buf, 256, "predicted position:(%3d,%3d)", predict_pt.x, predict_pt.y);
		cvPutText(img, buf, cvPoint(10, 30), &font, CV_RGB(0, 0, 0));
		sprintf_s(buf, 256, "current position :(%3d,%3d)", mousePosition.x, mousePosition.y);
		cvPutText(img, buf, cvPoint(10, 60), &font, CV_RGB(0, 0, 0));

		cvShowImage("kalman", img);
		int key = cvWaitKey(1000);
		if (key == 27){//esc   
			break;
		}
	}

	cvReleaseImage(&img);
	cvReleaseKalman(&kalman);
	return 0;
}