实现你的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论文看起来挺迷糊的，但是实现还是挺好实现的