OpenCV中Kalman(卡尔曼)滤波器的跟踪弹球模拟程序
最近在看《Learning OpenCV》中Kalman滤波器的内容，个人感觉“Kalman滤波器相关的一些数学知识”小节讲得挺好，能让人宏观上理解这个理论的意思，但是“Kalman方程”小节讲得就有点粗略了，让人不太理解。其实核心就在“Kalman方程”小节的那几个公式，建议大家去看控制方面的书籍，那里面有讲，而且很细致。
如果是自动化专业的本科学生，可以看看《现代控制理论》中的“状态估计”一章，如果自动化研究生，可参照导航与制导专业教材。
这部分知识得认真看，一个下午，什么都不干，估计就OK了。
下面是我写的一个Kalman（卡尔曼）滤波器的跟踪弹球模拟程序，大家在自己机器上运行下就知道这个程序的作用了。白色球是目标球，蓝色球是观测球，绿色球是kalman估计球，白球在四壁上来回撞击，蓝球观测，当白球撞壁反弹时，Kalman绿球被“甩”出弹壁，因为它以之前的状态估计白球还会往前走，但是它被甩出去后，它根据自己的模型和实际观测值重新更新模型，使自己重新较准确地跟踪白球。
我的是Opencv1.1+VS2005。
#include <stdio.h>
#include <cv.h>
#include <highgui.h>
#pragma comment(lib, "ml.lib")
#pragma comment(lib, "cv.lib")
#pragma comment(lib, "cvaux.lib")
#pragma comment(lib, "cvcam.lib")
#pragma comment(lib, "cxcore.lib")
#pragma comment(lib, "cxts.lib")
#pragma comment(lib, "highgui.lib")
#pragma comment(lib, "cvhaartraining.lib")
int main ()
{
cvNamedWindow("Kalman",1);
CvRandState random;//创建随机
cvRandInit(&random,0,1,-1,CV_RAND_NORMAL);
IplImage * image=cvCreateImage(cvSize(600,450),8,3);
CvKalman * kalman=cvCreateKalman(4,2,0);//状态变量4维，x、y坐标和在x、y方向上的速度，测量变量2维，x、y坐标
CvMat * xK=cvCreateMat(4,1,CV_32FC1);//初始化状态变量，坐标为（40,40），x、y方向初速度分别为10、10
xK->data.fl[0]=40.;
xK->data.fl[1]=40;
xK->data.fl[2]=10;
xK->data.fl[3]=10;
const float F[]={1,0,1,0,0,1,0,1,0,0,1,0,0,0,0,1};//初始化传递矩阵 [1 0 1 0]
// [0 1 0 1]
// [0 0 1 0]
// [0 0 0 1]
memcpy(kalman->transition_matrix->data.fl,F,sizeof(F));
CvMat * wK=cvCreateMat(4,1,CV_32FC1);//过程噪声
cvZero(wK);
CvMat * zK=cvCreateMat(2,1,CV_32FC1);//测量矩阵2维，x、y坐标
cvZero(zK);
CvMat * vK=cvCreateMat(2,1,CV_32FC1);//测量噪声
cvZero(vK);
cvSetIdentity( kalman->measurement_matrix, cvScalarAll(1) );//初始化测量矩阵H=[1 0 0 0]
// [0 1 0 0]
cvSetIdentity( kalman->process_noise_cov, cvScalarAll(1e-1) );/*过程噪声____设置适当数值，
增大目标运动的随机性，
但若设置的很大，则系统不能收敛，
即速度越来越快*/
cvSetIdentity( kalman->measurement_noise_cov, cvScalarAll(10) );/*观测噪声____故意将观测噪声设置得很大，
使之测量结果和预测结果同样存在误差*/
cvSetIdentity( kalman->error_cov_post, cvRealScalar(1) );/*后验误差协方差*/
cvRand( &random, kalman->state_post );
CvMat * mK=cvCreateMat(1,1,CV_32FC1); //反弹时外加的随机化矩阵
while(1){
cvZero( image );
cvRectangle(image,cvPoint(30,30),cvPoint(570,420),CV_RGB(255,255,255),2);//绘制目标弹球的“撞击壁”
const CvMat * yK=cvKalmanPredict(kalman,0);//计算预测位置
cvRandSetRange( &random, 0, sqrt( kalman->measurement_noise_cov->data.fl[0] ), 0 );
cvRand( &random, vK );//设置随机的测量误差
cvMatMulAdd( kalman->measurement_matrix, xK, vK, zK );//zK=H*xK+vK
cvCircle(image,cvPoint(cvRound(CV_MAT_ELEM(*xK,float,0,0)),cvRound(CV_MAT_ELEM(*xK,float,1,0))),
4,CV_RGB(255,255,255),2);//白圈，真实位置
cvCircle(image,cvPoint(cvRound(CV_MAT_ELEM(*yK,float,0,0)),cvRound(CV_MAT_ELEM(*yK,float,1,0))),
4,CV_RGB(0,255,0),2);//绿圈，预估位置
cvCircle(image,cvPoint(cvRound(CV_MAT_ELEM(*zK,float,0,0)),cvRound(CV_MAT_ELEM(*zK,float,1,0))),
4,CV_RGB(0,0,255),2);//蓝圈，观测位置
cvRandSetRange(&random,0,sqrt(kalman->process_noise_cov->data.fl[0]),0);
cvRand(&random,wK);//设置随机的过程误差
cvMatMulAdd(kalman->transition_matrix,xK,wK,xK);//xK=F*xK+wK
if(cvRound(CV_MAT_ELEM(*xK,float,0,0))<30){ //当撞击到反弹壁时，对应轴方向取反外加随机化
cvRandSetRange( &random, 0, sqrt(1e-1), 0 );
cvRand( &random, mK );
xK->data.fl[2]=10+CV_MAT_ELEM(*mK,float,0,0);
}
if(cvRound(CV_MAT_ELEM(*xK,float,0,0))>570){
cvRandSetRange( &random, 0, sqrt(1e-2), 0 );
cvRand( &random, mK );
xK->data.fl[2]=-(10+CV_MAT_ELEM(*mK,float,0,0));
}
if(cvRound(CV_MAT_ELEM(*xK,float,1,0))<30){
cvRandSetRange( &random, 0, sqrt(1e-1), 0 );
cvRand( &random, mK );
xK->data.fl[3]=10+CV_MAT_ELEM(*mK,float,0,0);
}
if(cvRound(CV_MAT_ELEM(*xK,float,1,0))>420){
cvRandSetRange( &random, 0, sqrt(1e-3), 0 );
cvRand( &random, mK );
xK->data.fl[3]=-(10+CV_MAT_ELEM(*mK,float,0,0));
}
printf("%f_____%f\n",xK->data.fl[2],xK->data.fl[3]);
cvShowImage("Kalman",image);
cvKalmanCorrect( kalman, zK );
if(cvWaitKey(100)=='e'){
break;
}
}
cvReleaseImage(&image);/*释放图像*/
cvDestroyAllWindows();
}
运行的效果图为
大家可以自己修改下代码中的几个协方差的数值，对比下效果。然后看看状态变量、观测变量、测量矩阵等维数，自然就明白Kalman滤波器的大致知识了。
如果大家有什么疑问或建议想法，欢迎联系！Q：530347188 Email：[email protected]
http://blog.csdn.net/guvcolie/article/details/7453680