首先来看一下OpenCV中关于Kalman滤波的结构和函数定义

CvKalman

Kalman 滤波器状态

结构 CvKalman 用来保存 Kalman 滤波器状态。它由函数 cvCreateKalman 创建，由函数f cvKalmanPredict 和 cvKalmanCorrect 更新，由 cvReleaseKalman 释放. 通常该结构是为标准 Kalman 所使用的 (符号和公式都借自非常优秀的 Kalman 教程 [Welch95]):

系统运动方程：

系统观测方程：

其中:

x_k(x_{k ? 1}) - 系统在时刻 k (k-1) 的状态向量 （state of the system at the moment k (k-1)）

z_k - 在时刻 k 的系统状态测量向量 （measurement of the system state at the moment k）

u_k - 应用于时刻 k 的外部控制 (external control applied at the moment k)

w_k 和 v_k 分别为正态分布的运动和测量噪声

p(w) ~ N(0,Q)

p(v) ~ N(0,R),

即,

Q - 运动噪声的相关矩阵，常量或变量

R - 测量噪声的相关矩阵，常量或变量

对标准 Kalman 滤波器，所有矩阵: A, B, H, Q 和 R 都是通过 cvCreateKalman 在分配结构 CvKalman 时初始化一次。但是，同样的结构和函数，通过在当前系统状态邻域中线性化扩展 Kalman 滤波器方程，可以用来模拟扩展 Kalman 滤波器，在这种情况下， A, B, H (也许还有 Q 和 R) 在每一步中都被更新。

CreateKalman

分配 Kalman 滤波器结构

dynam_params

状态向量维数

measure_params

测量向量维数

control_params

控制向量维数

函数 cvCreateKalman 分配 CvKalman 以及它的所有矩阵和初始参数

ReleaseKalman

释放 Kalman 滤波器结构

kalman

指向 Kalman 滤波器结构的双指针

函数 cvReleaseKalman 释放结构 CvKalman 和里面所有矩阵

KalmanPredict

估计后来的模型状态

kalman

Kalman 滤波器状态

control

控制向量 (uk), 如果没有外部控制 (control_params=0) 应该为 NULL

函数 cvKalmanPredict 根据当前状态估计后来的随机模型状态，并存储于 kalman->state_pre:

,

其中

x'_k 是预测状态 (kalman->state_pre),

x_{k ? 1} 是前一步的矫正状态 (kalman->state_post)，应该在开始的某个地方初始化，即缺省为零向量，

u_k 是外部控制(control 参数),

P'_k 是先验误差相关矩阵 (kalman->error_cov_pre)

P_{k ? 1} 是前一步的后验误差相关矩阵(kalman->error_cov_post),应该在开始的某个地方初始化，即缺省为单位矩阵.

函数返回估计得到的状态值

KalmanCorrect

调节模型状态

kalman

被更新的 Kalman 结构的指针

measurement

指向测量向量的指针，向量形式为 CvMat

函数 cvKalmanCorrect 在给定的模型状态的测量基础上，调节随机模型状态：

其中

z_k - 给定测量(mesurement parameter)

K_k - Kalman "增益" 矩阵

函数存储调节状态到 kalman->state_post 中并且输出时返回它。

下面实现了一个简单的跟踪小程序，直接给出程序源码：

void CSLAMApplicationView::OnEKFTracking()

{

               // 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 */

               IplImage* img = cvCreateImage( cvSize(500,500), 8, 3 );

               CvKalman* kalman = cvCreateKalman( 2, 1, 0 );/*状态向量为维，观测向量为维，无激励输入维*/

               // 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 ); /*随机填充数组*/

               // 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 }; /*状态转移矩阵*/

               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) );/*观测矩阵*/

               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 );

                               cvMatMulAdd( kalman->measurement_matrix, x_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 );

                               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();/*释放所有窗口*/

}

另外我的程序还实现了图片的打开和保存功能，具体也是参考了论坛的MFC中应用Opencv的帖子，不过我稍微改进了一下，不进行图片的缩放，显示源图像的大小：

首先是doc类定义CImage* m_Image;

CSLAMApplicationDoc::CSLAMApplicationDoc()

{

               m_Image=NULL;

}

CSLAMApplicationDoc::~CSLAMApplicationDoc()

{

               if(m_Image!=NULL)

               {

                               m_Image->Destroy();

                               delete m_Image;

               }

}

// CSLAMApplicationDoc 命令

BOOL CSLAMApplicationDoc::OnOpenDocument(LPCTSTR lpszPathName)

{

               if (!CDocument::OnOpenDocument(lpszPathName))

                               return FALSE;

               // TODO: Add your specialized creation code here

               m_Image=new CImage();

               m_Image->Load(lpszPathName);

               return TRUE;

}

BOOL CSLAMApplicationDoc::OnSaveDocument(LPCTSTR lpszPathName)

{

               // TODO: Add your specialized code here and/or call the base class

               m_Image->Save(lpszPathName);

               return CDocument::OnSaveDocument(lpszPathName);

}

// CSLAMApplicationView 绘制

void CSLAMApplicationView::OnDraw(CDC* pDC)

{

               CSLAMApplicationDoc* pDoc = GetDocument();

               ASSERT_VALID(pDoc);

               if (!pDoc)

                               return;

               // TODO: 在此处为本机数据添加绘制代码

               CImage *img=pDoc->m_Image;

               if(img!=NULL)

               {

                               CRect r;

                               GetClientRect (&r);

                               if(img->Width()<r.Width())

                               {

                                              r.right=img->Width();

                               }

                               if(img->Height()<r.Height())

                               {

                                              r.bottom=img->Height();

                               }

                               pDoc->m_Image->DrawToHDC(pDC->GetSafeHdc(),r);

               }