opencv 卡尔曼滤波器例子,自己修改过【转载】

转载 2012年03月28日 20:05:02
【转载】http://hi.baidu.com/lin65505578/blog/item/7c03a0d4210b68c7a144dfe6.html
  一、卡尔曼滤波器的理论解释
  
  http://blog.csdn.net/lindazhou2005/article/details/1534234(推荐)
  
  二、代码中一些随机数设置函数,在opencv中文网站上没有查到:
  
  cvRandInit()
  初始化CvRandState数据结构,可以选定随机分布的种类,并给定它种子,有两种情形
  cvRandInit(CvRandState数据结构,随机上界,随机下界,均匀分布参数,64bits种子的数字)
  cvRandInit(CvRandState数据结构,平均数,标准偏差,常态分布参数,64bits种子的数字)
  
  
  cvRandSetRange()
  修改CvRandState数据结构的参数内容,均匀分布的话可以每个信道的上下界常态分布可以修改每个通道的平均数,标准偏差.
  cvRandSetRange(CvRandState数据结构,均匀分布上界,均匀分布下界,目标信道数据)
  cvRandSetRange(CvRandState数据结构,常态分布平均数,常态分布标准偏差,目标信道数据)
  cvRand()
  将CvMat或IplImage数据结构随机化,用被设定过的CvRandState数据结构来随机.
  cvRand(CvRandState数据结构,CvMat或IplImage数据结构)
  cvbRand()
  将一维数组随机化,可以设定随机的长度
  cvbRand(RandState数据结构,Float型别数组名,随机的长度);
  
  http://blog.csdn.net/jiangdf/article/details/4553520
  
  #define cvMatMulAdd( src1, src2, src3, dst ) cvGEMM( src1, src2, 1, src3, 1, dst, 0 )
  
  三、
  
  1、情景说明:
  
  假设有一个做圆周运动的点,就像一辆在赛道上行驶的汽车,汽车以恒定的速度行驶。使用kalman滤波器跟踪这个汽车。程序运行结果,红圈代表理想运动汽车,,黄圈代表kalman滤波器预测汽车的位置,白圈代表观测得到汽车的位置,蓝圈代表kalman滤波器获得汽车的位置(速度这一分量,可以观察图中圆的运动速度)。
  
  2、代码:
  
  
  #include 
  #include 
  #include 
  
  int main (int argc,int ** argv)
  {
   // Initialize Kalman filter object, window, number generator, etc
   cvNamedWindow( "Kalman", 1 );//创建窗口,当为的时候,表示窗口大小自动设定
   CvRandState rng;
   //cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );/* CV_RAND_UNI 指定为均匀分布类型、随机数种子为-1 *///怎么接下来又改为高斯分布
  
   cvRandInit(&rng,0,0.1,-1,CV_RAND_NORMAL);//自己添加,原函数中先设为均匀分布再改回,觉得没有必要
   IplImage* img = cvCreateImage( cvSize(500,500), 8, 3 );
   CvKalman* kalman = cvCreateKalman( 2, 1, 0 );/*状态向量为2维,观测向量为1维,无激励输入维*/
  
   // State is phi, delta_phi - angle and angular velocity
   // Initialize with random guess
   CvMat* x_k = cvCreateMat( 2, 1, CV_32FC1 );/*创建行列、元素类型为CV_32FC1,元素为位单通道浮点类型矩阵。*/
   //cvRandSetRange( &rng, 0, 0.1, 0 );/*设置随机数范围,随机数服从正态分布,均值为,均方差为.1,通道个数为*/
   //rng.disttype = CV_RAND_NORMAL;
   //cvRand( &rng, x_k ); /*随机填充数组*/
   x_k->data.fl[0]=0.;//设起始角度为0;//自己修改过的例子中x_k为汽车沿圆周运动的参数,去掉了噪声,在后面与kalman
  
   //滤波器校正后的运动参数对比
   x_k->data.fl[1]=0.05f;//设角速度为1,弧度值
  
   // Process noise
   CvMat* w_k = cvCreateMat( 2, 1, CV_32FC1 );
  
   // Measurements, only one parameter for angle
   CvMat* z_k = cvCreateMat( 1, 1, CV_32FC1 );/*定义观测变量*/
   cvZero( z_k ); /*矩阵置零*/
  
   // Transition matrix F describes model parameters at and k and k+1
   const float F[] = { 1, 1, 0, 1 }; /*状态转移矩阵*///F 矩阵实际上应该是2*2的矩阵[1,1;0,1]
   memcpy( kalman->transition_matrix->data.fl, F, sizeof(F));
   /*初始化转移矩阵,行列,具体见CvKalman* kalman = cvCreateKalman( 2, 1, 0 );*/
  
   // Initialize other Kalman parameters
   cvSetIdentity( kalman->measurement_matrix, cvRealScalar(1) );/*观测矩阵*///观测矩阵也是1*2 [1,0],因为只能测得汽车的位置
   cvSetIdentity( kalman->process_noise_cov, cvRealScalar(1e-5) );/*过程噪声*/
   cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1e-1) );/*观测噪声*/
   cvSetIdentity( kalman->error_cov_post, cvRealScalar(1) );/*后验误差协方差*/
  
   // Choose random initial state
   cvRand( &rng, kalman->state_post );/*初始化状态向量*/
  
   // Make colors
   CvScalar yellow = CV_RGB(255,255,0);/*依次为红绿蓝三色*/
   CvScalar white = CV_RGB(255,255,255);
   CvScalar red = CV_RGB(255,0,0);
  
   while( 1 ){
   // Predict point position
   const CvMat* y_k = cvKalmanPredict( kalman, 0 );/*激励项输入为*/
  
   // Generate Measurement (z_k)
   cvRandSetRange( &rng, 0, sqrt( kalman->measurement_noise_cov->data.fl[0] ), 0 );/*设置观测噪声*/
   cvRand( &rng, z_k );//此时的z_k并非测量值,只是为观测噪声,其实为理解方便应该设一的独立的变量
   cvMatMulAdd( kalman->measurement_matrix, x_k, z_k, z_k );//z_k为测量值
  
   // Update Kalman filter state
   cvKalmanCorrect( kalman, z_k );
  
   //// Apply the transition matrix F and apply "process noise" w_k
   //cvRandSetRange( &rng, 0, sqrt( kalman->process_noise_cov->data.fl[0] ), 0 );/*设置正态分布过程噪声*/
   //cvRand( &rng, w_k );
   //cvMatMulAdd( kalman->transition_matrix, x_k, w_k, x_k );//
  
   // Plot Points
   cvZero( img );/*创建图像*/
   // Yellow is observed state 黄色是观测值
   //cvCircle(IntPtr, Point, Int32, MCvScalar, Int32, LINE_TYPE, Int32)
   //对应于下列其中,shift为数据精度
   //cvCircle(img, center, radius, color, thickness, lineType, shift)
   //绘制或填充一个给定圆心和半径的圆
   cvCircle( img, 
   cvPoint( cvRound(img->width/2 + img->width/3*cos(z_k->data.fl[0])),
   cvRound( img->height/2 - img->width/3*sin(z_k->data.fl[0])) ), 
   4, yellow );
   // White is the predicted state via the filter
   cvCircle( img, 
   cvPoint( cvRound(img->width/2 + img->width/3*cos(y_k->data.fl[0])),
   cvRound( img->height/2 - img->width/3*sin(y_k->data.fl[0])) ), 
   4, white, 2 );
   // Red is the real state
   cvCircle( img, 
   cvPoint( cvRound(img->width/2 + img->width/3*cos(x_k->data.fl[0])),
   cvRound( img->height/2 - img->width/3*sin(x_k->data.fl[0])) ),
   4, red );
   CvScalar bule=cvScalar(255,0,0,0);
   cvCircle( img, 
   cvPoint( cvRound(img->width/2 + img->width/3*cos(kalman->state_post->data.fl[0])),
   cvRound( img->height/2 - img->width/3*sin(kalman->state_post->data.fl[0])) ),
   4, bule ,2);
  
   // Apply the transition matrix F and apply "process noise" w_k
   //cvRandSetRange( &rng, 0, sqrt( kalman->process_noise_cov->data.fl[0] ), 0 );/*设置正态分布过程噪声*/
   //cvRand( &rng, w_k );
   cvMatMulAdd( kalman->transition_matrix, x_k, NULL, x_k );//
  
   CvFont font;
   cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX,0.5f,0.5f,0,1,8);
   cvPutText(img,"Yellow:observe",cvPoint(0,20),&font,cvScalar(0,0,255));
   cvPutText(img,"While:predict",cvPoint(0,40),&font,cvScalar(0,0,255));
   cvPutText(img,"Red:real",cvPoint(0,60),&font,cvScalar(0,0,255));
   cvPutText(img,"Press Esc to Exit...",cvPoint(0,80),&font,cvScalar(255,255,255));
   cvShowImage( "Kalman", img ); 
  
   // Exit on esc key
   if(cvWaitKey(100) == 27) 
   break;
   }
   cvReleaseImage(&img);/*释放图像*/
   cvReleaseKalman(&kalman);/*释放kalman滤波对象*/
   cvDestroyAllWindows();/*释放所有窗口*/
  }

相关文章推荐

行人检测系统中的行人特征及检测方法

http://blog.csdn.net/perry_pku/article/details/52370519 1.行人特征 行人特征描述子可以分为三类:底层特征,混合特征和基于学习的...

OpenCV读入图片序列进行HOG行人检测并保存为视频

OpenCV读入图片序列进行HOG行人检测并保存为视频
  • masikkk
  • masikkk
  • 2013年11月13日 21:42
  • 6388

OpenCV自学笔记16. 卡尔曼滤波器小例子

卡尔曼滤波器的小例子(未完)卡尔曼滤波器由Rudolf Kalman在20世纪50年代末提出的算法,在导航系统上得到了广泛的应用,据说在阿波罗登月的过程中,就使用了这种算法。对卡尔曼滤波原理的理解可见...

卡尔曼滤波器 opencv

背景: 卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全及包含噪声的测量中,估计动态系统的状态。卡尔曼滤波的一个典型实例是从一组有限的,包含噪声的,对物体位置的观...

OpenCV 卡尔曼滤波器的使用

原文地址:http://news.qiyeku.com/news_707688.html 首先来看一下OpenCV中关于Kalman滤波的结构和函数定义 CvKal...
  • apsvvfb
  • apsvvfb
  • 2013年10月08日 20:20
  • 719

很形象的卡尔曼滤波器原理介绍(转载)

在学习卡尔曼滤波器之前,首先看看为什么叫“卡尔曼”。跟其他著名的理论(例如傅立叶变换,泰勒级数等等)一样,卡尔曼也是一个人的名字,而跟他们不同的是,他是个现代人! 1. 卡尔曼全名Rudolf Em...

人工智能之卡尔曼滤波器浅析及matlab实战(很好的入门)

原文作者:Liu_LongPo 链接:http://blog.csdn.net/llp1992/article/details/45370025   卡尔曼滤波器是一种利用线性系统状态方程...

kalman 卡尔曼滤波器

理论基础 为什么需要协方差? 标准差和方差一般是用来描述一维数据的,但现实生活中我们常常会遇到含有多维数据的数据集,最简单的是大家上学时免不了要统计多个学科的考试成绩。面对这样的数据集...

浅谈卡尔曼滤波器

1795年,高斯为了测定行星轨道提出了最小二乘估计法。 1942年,维纳为了解决火力控制系统精度跟踪问题,提出了维纳滤波理论。 维纳滤波理论首次将数理统计理论与线性理论有机结合,形成了对随机信号最...

【初识】卡尔曼滤波器 Kalman Filter

卡尔曼滤波器 Kalman Filter (zz) 转自:http://carpa.bokee.com/4725695.html 关键词: 卡尔曼滤波器 Kalman Filter 在学...
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:opencv 卡尔曼滤波器例子,自己修改过【转载】
举报原因:
原因补充:

(最多只允许输入30个字)