实现你的kalman

本文结合opencv的Mat数据类型，实现了kalman跟踪，具体理论知识可以参考博客：http://blog.csdn.net/baimafujinji/article/details/50646814

#ifndef _KALMAN_H_
#define _KALMAN_H_

#include <iostream>
#include <opencv2\opencv.hpp>
using namespace std;
using namespace cv;

class KALMAN
{
public:
	KALMAN(int state_size, int mea_size);
	~KALMAN();

public:
	Mat statePre;//预测状态矩阵(x'(k)) x(k) = A*x(k - 1) + B * u(k)
	Mat statePost;//状态估计修正矩阵(x(k)) x(k) = x'(k) + K(k)*(z(k) - H * x'(k)) ： 1 * 8
	Mat transitionMatrix;//转移矩阵(A)  ： 8 * 8
	Mat controMatrix;//控制矩阵(B)
	Mat measurementMatrix;//测量矩阵(H) ：4 * 8
	Mat processNoiseCov;//预测模型噪声协方差矩阵(Q) ：8 * 8
	Mat measurementNoiseCov;//测量噪声协方差矩阵(R)  ： 4 * 4
	Mat errorCovPre;//转移噪声矩阵(P'(k)) p'(k) = A * p(k - 1) * At + Q 
	Mat K;//kalman增益矩阵 K = p'(k) * Ht * inv(H * p'(k) * Ht + R)
	Mat errorCovPost;//转移噪声修正矩阵(p(k)) p(k) = (I - K(k) * H) * p'(k)  ： 8 * 8

public:
	void init();
	void update(Mat Y);
	Mat predicted(Mat Y);


};


#endif;

#include "kalman.h"


KALMAN::KALMAN(int state_size,int mea_size)
{
	transitionMatrix = Mat::zeros(state_size,state_size,CV_32F);
	measurementMatrix = Mat::zeros(mea_size,state_size,CV_32F);
	processNoiseCov = Mat::zeros(state_size,state_size,CV_32F);
	measurementNoiseCov = Mat::zeros(mea_size,mea_size,CV_32F);
	errorCovPre = Mat::zeros(state_size, state_size, CV_32F);
	errorCovPost = Mat::zeros(state_size, state_size, CV_32F);
	statePost = Mat::zeros(state_size,1,CV_32F);
	statePre = Mat::zeros(state_size,1,CV_32F);
	K = Mat::zeros(state_size,mea_size,CV_32F);
}

KALMAN::~KALMAN()
{

}

void KALMAN::init()
{
	setIdentity(measurementMatrix,Scalar::all(1));//观测矩阵的初始化;
	setIdentity(processNoiseCov,Scalar::all(1e-5));//模型本身噪声协方差矩阵初始化;
	setIdentity(measurementNoiseCov,Scalar::all(1e-1));//测量噪声的协方差矩阵初始化
	setIdentity(errorCovPost,Scalar::all(1));//转移噪声修正矩阵初始化
	randn(statePost,Scalar::all(0),Scalar::all(5));//kalaman状态估计修正矩阵初始化
}

void KALMAN::update(Mat Y)
{
	K = errorCovPre * (measurementMatrix.t()) * ((measurementMatrix * errorCovPre * measurementMatrix.t() + measurementNoiseCov).inv());
	statePost = statePre + K * (Y - measurementMatrix * statePre);
	errorCovPost = errorCovPre - K * measurementMatrix * errorCovPre;
}

Mat KALMAN::predicted(Mat Y)
{

	statePre = transitionMatrix * statePost;
	errorCovPre = transitionMatrix * errorCovPost * transitionMatrix.t() + processNoiseCov;

	update(Y);


	return statePost;
}

#include "kalman.h"


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

Point mousePosition = Point(winWidth >> 1, winHeight >> 1);

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

void main()
{
	int state_size = 4;
	int mea_size = 2;
	KALMAN kalman(state_size,mea_size);

	kalman.init();
	kalman.transitionMatrix = *(Mat_<float>(4, 4) <<
		1, 0, 1, 0,
		0, 1, 0, 1,
		0, 0, 1, 0,
		0, 0, 0, 1);//元素导入矩阵，按行; 

	Mat showImg(winWidth, winHeight, CV_8UC3);
	Mat measurement(mea_size,1,CV_32F);




	for (;;)
	{
		setMouseCallback("Kalman", mouseEvent);
		showImg.setTo(0);

		Point statePt = Point((int)kalman.statePost.at<float>(0), (int)kalman.statePost.at<float>(1));

		//3.update measurement  
		measurement.at<float>(0) = (float)mousePosition.x;
		measurement.at<float>(1) = (float)mousePosition.y;

		//2.kalman prediction     
		Mat prediction = kalman.predicted(measurement);
		Point predictPt = Point((int)prediction.at<float>(0), (int)prediction.at<float>(1));


		//randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));  
		//state = KF.transitionMatrix*state + processNoise;  
		//draw  
		circle(showImg, statePt, 5, CV_RGB(255, 0, 0), 1);//former point  
		circle(showImg, predictPt, 5, CV_RGB(0, 255, 0), 1);//predict point  
		circle(showImg, mousePosition, 5, CV_RGB(0, 0, 255), 1);//ture point  


		//          CvFont font;//字体  
		//          cvInitFont(&font, CV_FONT_HERSHEY_SCRIPT_COMPLEX, 0.5f, 0.5f, 0, 1, 8);  
		char buf[256];
		sprintf_s(buf, 256, "Green:predicted position:(%3d,%3d)", predictPt.x, predictPt.y);
		//putText(showImg, "Red: Former Point", cvPoint(10, 30), FONT_HERSHEY_SIMPLEX, 1, Scalar::all(255));
		putText(showImg, buf, cvPoint(10, 60), FONT_HERSHEY_SIMPLEX, 1, Scalar::all(255));
		sprintf_s(buf, 256, "true position:(%3d,%3d)", mousePosition.x, mousePosition.y);
		putText(showImg, buf, cvPoint(10, 90), FONT_HERSHEY_SIMPLEX, 1, Scalar::all(255));

		imshow("Kalman", showImg);
		int key = waitKey(3);
		if (key == 27)
		{
			break;
		}
	}
}

kalman论文看起来挺迷糊的，但是实现还是挺好实现的