kalman滤波器例子

（转自：http://blog.csdn.net/chary8088/article/details/24935559）

一、卡尔曼滤波器的理论解释

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、代码：

[cpp]  view plain copy

1. #include <cv.h>  
2. #include <highgui.h>  
3. #include <stdio.h>  
4. int main (int argc,int ** argv)  
5. {  
6.  // Initialize Kalman filter object, window, number generator, etc  
7.  cvNamedWindow( "Kalman", 1 );//创建窗口，当为的时候，表示窗口大小自动设定  
8.  CvRandState rng;  
9.  //cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );/* CV_RAND_UNI 指定为均匀分布类型、随机数种子为-1 *///怎么接下来又改为高斯分布  
10.  cvRandInit(&rng,0,0.1,-1,CV_RAND_NORMAL);//自己添加，原函数中先设为均匀分布再改回，觉得没有必要  
11.  IplImage* img = cvCreateImage( cvSize(500,500), 8, 3 );  
12.  CvKalman* kalman = cvCreateKalman( 2, 1, 0 );/*状态向量为2维，观测向量为1维，无激励输入维*/  
13.  // State is phi, delta_phi - angle and angular velocity  
14.  // Initialize with random guess  
15.  CvMat* x_k = cvCreateMat( 2, 1, CV_32FC1 );/*创建行列、元素类型为CV_32FC1，元素为位单通道浮点类型矩阵。*/  
16.  //cvRandSetRange( &rng, 0, 0.1, 0 );/*设置随机数范围，随机数服从正态分布，均值为，均方差为.1，通道个数为*/  
17.  //rng.disttype = CV_RAND_NORMAL;  
18.  //cvRand( &rng, x_k ); /*随机填充数组*/  
19.  x_k->data.fl[0]=0.;//设起始角度为0；//自己修改过的例子中x_k为汽车沿圆周运动的参数，去掉了噪声，在后面与kalman  
20. //滤波器校正后的运动参数对比  
21.  x_k->data.fl[1]=0.05f;//设角速度为1,弧度值  
22.  // Process noise  
23.  CvMat* w_k = cvCreateMat( 2, 1, CV_32FC1 );  
24.  // Measurements, only one parameter for angle  
25.  CvMat* z_k = cvCreateMat( 1, 1, CV_32FC1 );/*定义观测变量*/  
26.  cvZero( z_k ); /*矩阵置零*/  
27.  // Transition matrix F describes model parameters at and k and k+1  
28.  const float F[] = { 1, 1, 0, 1 }; /*状态转移矩阵*///F 矩阵实际上应该是2*2的矩阵[1,1;0,1]  
29.  memcpy( kalman->transition_matrix->data.fl, F, sizeof(F));  
30.  /*初始化转移矩阵，行列，具体见CvKalman* kalman = cvCreateKalman( 2, 1, 0 );*/  
31.  // Initialize other Kalman parameters  
32.  cvSetIdentity( kalman->measurement_matrix, cvRealScalar(1) );/*观测矩阵*///观测矩阵也是1*2 [1,0],因为只能测得汽车的位置  
33.  cvSetIdentity( kalman->process_noise_cov, cvRealScalar(1e-5) );/*过程噪声*/  
34.  cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1e-1) );/*观测噪声*/  
35.  cvSetIdentity( kalman->error_cov_post, cvRealScalar(1) );/*后验误差协方差*/  
36.  // Choose random initial state  
37.  cvRand( &rng, kalman->state_post );/*初始化状态向量*/  
38.  // Make colors  
39.  CvScalar yellow = CV_RGB(255,255,0);/*依次为红绿蓝三色*/  
40.  CvScalar white = CV_RGB(255,255,255);  
41.  CvScalar red = CV_RGB(255,0,0);  
42.  while( 1 ){  
43.   // Predict point position  
44.   const CvMat* y_k = cvKalmanPredict( kalman, 0 );/*激励项输入为*/  
45.   // Generate Measurement (z_k)  
46.   cvRandSetRange( &rng, 0, sqrt( kalman->measurement_noise_cov->data.fl[0] ), 0 );/*设置观测噪声*/   
47.   cvRand( &rng, z_k );//此时的z_k并非测量值，只是为观测噪声，其实为理解方便应该设一的独立的变量  
48.   cvMatMulAdd( kalman->measurement_matrix, x_k, z_k, z_k );//z_k为测量值  
49.   // Update Kalman filter state  
50.   cvKalmanCorrect( kalman, z_k );  
51.    Apply the transition matrix F and apply "process noise" w_k  
52.   //cvRandSetRange( &rng, 0, sqrt( kalman->process_noise_cov->data.fl[0] ), 0 );/*设置正态分布过程噪声*/  
53.   //cvRand( &rng, w_k );  
54.   //cvMatMulAdd( kalman->transition_matrix, x_k, w_k, x_k );//  
55.   // Plot Points  
56.   cvZero( img );/*创建图像*/  
57.   // Yellow is observed state 黄色是观测值  
58.   //cvCircle(IntPtr, Point, Int32, MCvScalar, Int32, LINE_TYPE, Int32)  
59.   //对应于下列其中，shift为数据精度  
60.   //cvCircle(img, center, radius, color, thickness, lineType, shift)  
61.   //绘制或填充一个给定圆心和半径的圆  
62.   cvCircle( img,   
63.    cvPoint( cvRound(img->width/2 + img->width/3*cos(z_k->data.fl[0])),  
64.    cvRound( img->height/2 - img->width/3*sin(z_k->data.fl[0])) ),   
65.    4, yellow );  
66.   // White is the predicted state via the filter  
67.   cvCircle( img,   
68.    cvPoint( cvRound(img->width/2 + img->width/3*cos(y_k->data.fl[0])),  
69.    cvRound( img->height/2 - img->width/3*sin(y_k->data.fl[0])) ),   
70.    4, white, 2 );  
71.   // Red is the real state  
72.   cvCircle( img,   
73.   cvPoint( cvRound(img->width/2 + img->width/3*cos(x_k->data.fl[0])),  
74.    cvRound( img->height/2 - img->width/3*sin(x_k->data.fl[0])) ),  
75.    4, red );  
76.   CvScalar bule=cvScalar(255,0,0,0);  
77.   cvCircle( img,   
78.    cvPoint( cvRound(img->width/2 + img->width/3*cos(kalman->state_post->data.fl[0])),  
79.    cvRound( img->height/2 - img->width/3*sin(kalman->state_post->data.fl[0])) ),  
80.    4, bule ,2);  
81.   // Apply the transition matrix F and apply "process noise" w_k  
82.   //cvRandSetRange( &rng, 0, sqrt( kalman->process_noise_cov->data.fl[0] ), 0 );/*设置正态分布过程噪声*/  
83.   //cvRand( &rng, w_k );  
84.   cvMatMulAdd( kalman->transition_matrix, x_k, NULL, x_k );//  
85.   CvFont font;  
86.   cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX,0.5f,0.5f,0,1,8);  
87.   cvPutText(img,"Yellow:observe",cvPoint(0,20),&font,cvScalar(0,0,255));  
88.   cvPutText(img,"While:predict",cvPoint(0,40),&font,cvScalar(0,0,255));  
89.   cvPutText(img,"Red:real",cvPoint(0,60),&font,cvScalar(0,0,255));  
90.   cvPutText(img,"Press Esc to Exit...",cvPoint(0,80),&font,cvScalar(255,255,255));  
91.   cvShowImage( "Kalman", img );    
92.   // Exit on esc key  
93.   if(cvWaitKey(100) == 27)   
94.    break;  
95.  }  
96.  cvReleaseImage(&img);/*释放图像*/  
97.  cvReleaseKalman(&kalman);/*释放kalman滤波对象*/  
98.  cvDestroyAllWindows();/*释放所有窗口*/  
99. }  

3、平台：opencv2.1 vs2008
4、实验结果：

5、分析

实验中有两个现象

一开始汽车的真实位置和卡尔曼滤波器得到的结果差别比较大。因为汽车一开始的位置为0，而给卡尔曼的滤波器设置的最初状态时一个高斯分布的随机值。但越到后来，汽车的真实位置和卡尔曼滤波器得到的结果差别变小；

蓝圈位置和白圈位置更接近，而黄圈和蓝圈的位置总是差别较大。因为过程协方差为1e-1，而观测协方差为1e-5（蓝色部分代码），所以卡尔曼滤波器得到的结果和观测结果更接近，即蓝圈位置和白圈位置更接近。如果改变过程协方差的大小会出现不同的结果。