kalman跟踪的实现

#ifndef _KALMANFILTER_
#define _KALMANFILTER_
class CKalman
{
public:
	CKalman(int D, int M, int C);
	~CKalman();
	void KalmanPredict(double * control);
	void KalmanCorrect(double * measurement);
	void set_transition_matrix(const double*transition_matrix);
public:
	int MP;  //number of measure vector dimensions 
	int DP;  //number of state   vector dimensions 
	int CP;  //number of control vector dimensions 

	double* state_pre;            //[DP * 1]  
	double* state_post;           //[DP * 1]  
	double* transition_matrix;    //[DP * DP]  
	double* control_matrix;       //[DP * CP] if (CP > 0)  
	double* measurement_matrix;   //[MP * DP] 
	double* process_noise_cov;    //[DP * DP] 
	double* measurement_noise_cov;//[MP * MP] 
	double* error_cov_pre;        //[DP * DP] 
	double* error_cov_post;       //[DP * DP] 
	double* gain;                 //[DP * MP] 
};

#endif

#include "stdafx.h"
#include "kalmanfilter.h"  
#include "Matrix.h"   
#include<memory>
#include<assert.h>
using namespace std;


CKalman::CKalman(int D, int M, int C){
	if (D <= 0 || M <= 0){
		
		assert(false);// "state and measurement vectors must have positive number of dimensions! ");
	}
	if (C < 0)
	{
		assert(false);// "No Control!");
	}
	DP = D;
	MP = M;
	CP = C;
	state_pre = new double[DP * 1];
	memset(state_pre, 0, sizeof(double)*DP);
	state_post = new double[DP * 1];
	memset(state_post, 0, sizeof(*state_post));
	transition_matrix = new double[DP * DP];
	memset(transition_matrix, 0, sizeof(double)*DP*DP);
	process_noise_cov = new double[DP * DP];
	memset(process_noise_cov, 0, sizeof(double)*DP*DP);
	measurement_matrix = new double[MP * DP];
	memset(measurement_matrix, 0, sizeof(double)*MP*DP);
	measurement_noise_cov = new double[MP * MP];
	memset(measurement_noise_cov, 0, sizeof(double)*MP*MP);
	error_cov_pre = new double[DP * DP];
	memset(error_cov_pre, 0, sizeof(double)*DP*DP);
	error_cov_post = new double[DP * DP];
	memset(error_cov_post, 0, sizeof(double)*DP*DP);
	gain = new double[DP * MP];
	memset(gain, 0, sizeof(double)*DP*MP);
	if (CP > 0)
	{
		control_matrix = new double[DP * CP];
		memset(control_matrix, 0, sizeof(double)*DP*CP);
	}
}

void CKalman::set_transition_matrix(const double*trans_matrix)
{
	for (int i = 0; i < DP*DP; i++)
	{
		transition_matrix[i] = trans_matrix[i];
	}
}

CKalman::~CKalman(){
	delete[] state_pre;
	delete[] state_post;
	delete[] transition_matrix;
	if (CP >0){
		delete[] control_matrix;
	}
	delete[] measurement_matrix;
	delete[] process_noise_cov;
	delete[] measurement_noise_cov;
	delete[] error_cov_pre;
	delete[] gain;
	delete[] error_cov_post;
}

void CKalman::KalmanPredict(double * control){

	CMatrix matrix;
	/* update the state */
	/* x'(k) = A*x(k) */
	matrix.Mul(transition_matrix, DP, DP, state_post, DP, 1, state_pre);

	if (control && CP > 0){
		/* x'(k) = x'(k) + B*u(k) */
		double * temp1 = new double[DP * 1];
		matrix.Mul(control_matrix, DP, CP, control, CP, 1, temp1);
		matrix.Add(state_pre, temp1, state_pre, DP, 1);
		delete[] temp1;
		temp1 = NULL;
	}

	/* update error covariance matrices */
	/* temp2 = A*P(k) */
	double * temp2 = new double[DP * DP];
	matrix.Mul(transition_matrix, DP, DP, error_cov_post, DP, DP, temp2);

	/* P'(k) = temp2*At + Q */
	double * temp3 = new double[DP * DP];
	double * temp4 = new double[DP * DP];
	matrix.Transpose(transition_matrix, DP, DP, temp3);
	matrix.Mul(temp2, DP, DP, temp3, DP, DP, temp4);
	matrix.Add(temp4, process_noise_cov, error_cov_pre, DP, DP);
	delete[] temp2;  temp2 = NULL;
	delete[] temp3;  temp3 = NULL;
	delete[] temp4;  temp4 = NULL;
}

void CKalman::KalmanCorrect(double * measurement){

	if (!measurement){
		assert(false);// , "Measurement is Null!!!");
	}
	CMatrix matrix;

	/* temp1 = Ht*/
	double * temp1 = new double[DP * MP];
	matrix.Transpose(measurement_matrix, MP, DP, temp1);
	/* temp2 = P'(k) * temp1 */
	double * temp2 = new double[DP * MP];
	matrix.Mul(error_cov_pre, DP, DP, temp1, DP, MP, temp2);

	/* temp3 = H*temp2 + R */
	double * temp3 = new double[MP * MP];
	matrix.Mul(measurement_matrix, MP, DP, temp2, DP, MP, temp3);
	matrix.Add(temp3, measurement_noise_cov, temp3, MP, MP);

	/* temp4 = inv(temp3) */
	double * temp4 = new double[MP * MP];
	matrix.ContraryMatrix(temp3, temp4, MP);

	/* K(k) = temp2 * temp4  */
	matrix.Mul(temp2, DP, MP, temp4, MP, MP, gain);
	delete[] temp1;  temp1 = NULL;
	delete[] temp2;  temp2 = NULL;
	delete[] temp3;  temp3 = NULL;
	delete[] temp4;  temp4 = NULL;

	//update state_post   
	/* temp5 = z(k) - H*x'(k) */
	double * temp5 = new double[MP * 1];
	matrix.Mul(measurement_matrix, MP, DP, state_pre, DP, 1, temp5);
	matrix.Sub(measurement, temp5, temp5, MP, 1);
	/* x(k) = x'(k) + K(k)*temp5 */
	double * temp6 = new double[DP * 1];
	matrix.Mul(gain, DP, MP, temp5, MP, 1, temp6);
	matrix.Add(state_pre, temp6, state_post, DP, 1);
	delete[] temp5;  temp5 = NULL;
	delete[] temp6;  temp6 = NULL;

	//update error_cov_post   
	/* P(k) = P'(k) - K(k)*H* P'(k) */
	double * temp7 = new double[MP * DP];
	double * temp8 = new double[DP * DP];
	matrix.Mul(measurement_matrix, MP, DP, error_cov_pre, DP, DP, temp7);
	matrix.Mul(gain, DP, MP, temp7, MP, DP, temp8);
	matrix.Sub(error_cov_pre, temp8, error_cov_post, DP, DP);
	delete[] temp7;  temp7 = NULL;
	delete[] temp8;  temp8 = NULL;
}

调用

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

#include "stdafx.h"
#include"kalmanfilter.h"
#include<time.h>
#include<cstdlib>
#include <cv.h>  
#include <cxcore.h>  
#include <highgui.h> 

using namespace std;


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



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

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


int _tmain(int argc, _TCHAR* argv[])
{
	time_t t;
	srand(time(&t));

	//1.kalman filter setup  
	const int stateNum = 4;
	const int measureNum = 2;
	CKalman kf(stateNum, measureNum, 0);//state(x,y,detaX,detaY) 
	double A[] = {//transition matrix  
		1, 0, 1, 0,
		0, 1, 0, 1,
		0, 0, 1, 0,
		0, 0, 0, 1
	};
	kf.set_transition_matrix(A);
	
	kf.measurement_matrix[0]  = 1;//for x,x is an observation
	kf.measurement_matrix[5] =  1;//for y,y is an observation

	kf.measurement_noise_cov[0] = 1e1;
	kf.measurement_noise_cov[3] = 1e1;

	kf.process_noise_cov[0] = 1e-1;//diag
	kf.process_noise_cov[5] = 1e-1;
	kf.process_noise_cov[10] = 1e-1;
	kf.process_noise_cov[15] = 1e-1;
	
	int nn = winHeight > winWidth ? winWidth : winHeight;
	kf.state_post[0] = double(rand()) / RAND_MAX*nn;//x
	kf.state_post[1] = double(rand()) / RAND_MAX*nn;//y    
	kf.state_post[2]=rand();//deltaX
	kf.state_post[3]=rand();//deltaY
	
	kf.error_cov_post[0]  = 1;//diag
	kf.error_cov_post[5] = 1;
	kf.error_cov_post[10] = 1;
	kf.error_cov_post[15] = 1;


	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  
		kf.KalmanPredict(NULL);
		CvPoint predict_pt = cvPoint((int)kf.state_pre[0], (int)kf.state_pre[1]);

		//3.update measurement  
		
		double measure[2];
		measure[0] = double(mousePosition.x);
		measure[1] = double(mousePosition.y);
		//4.update  
		kf.KalmanCorrect(measure);
		



		//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(3);
		if (key == 27){//esc     
			break;
		}
	}

	cvReleaseImage(&img);
	
	return 0;

}