Opencv中卡尔曼滤波器的使用

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

CvKalman

Kalman 滤波器状态

typedef struct CvKalman
{
    int MP;                     /* 测量向量维数 */
    int DP;                     /* 状态向量维数 */
    int CP;                     /* 控制向量维数 */

    /* 向后兼容字段 */
#if 1
    float* PosterState;         /* =state_pre->data.fl */
    float* PriorState;          /* =state_post->data.fl */
    float* DynamMatr;           /* =transition_matrix->data.fl */
    float* MeasurementMatr;     /* =measurement_matrix->data.fl */
    float* MNCovariance;        /* =measurement_noise_cov->data.fl */
    float* PNCovariance;        /* =process_noise_cov->data.fl */
    float* KalmGainMatr;        /* =gain->data.fl */
    float* PriorErrorCovariance;/* =error_cov_pre->data.fl */
    float* PosterErrorCovariance;/* =error_cov_post->data.fl */
    float* Temp1;               /* temp1->data.fl */
    float* Temp2;               /* temp2->data.fl */
#endif

    CvMat* state_pre;           /* 预测状态 (x'(k)): 
                                    x(k)=A*x(k-1)+B*u(k) */
    CvMat* state_post;          /* 矫正状态 (x(k)):
                                    x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) */
    CvMat* transition_matrix;   /* 状态传递矩阵 state transition matrix (A) */
    CvMat* control_matrix;      /* 控制矩阵 control matrix (B)
                                   (如果没有控制，则不使用它)*/
    CvMat* measurement_matrix;  /* 测量矩阵 measurement matrix (H) */
    CvMat* process_noise_cov;   /* 过程噪声协方差矩阵
                                        process noise covariance matrix (Q) */
    CvMat* measurement_noise_cov; /* 测量噪声协方差矩阵
                                          measurement noise covariance matrix (R) */
    CvMat* error_cov_pre;       /* 先验误差计协方差矩阵
                                        priori error estimate covariance matrix (P'(k)):
                                     P'(k)=A*P(k-1)*At + Q)*/
    CvMat* gain;                /* Kalman 增益矩阵 gain matrix (K(k)):
                                    K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)*/
    CvMat* error_cov_post;      /* 后验错误估计协方差矩阵
                                        posteriori error estimate covariance matrix (P(k)):
                                     P(k)=(I-K(k)*H)*P'(k) */
    CvMat* temp1;               /* 临时矩阵 temporary matrices */
    CvMat* temp2;
    CvMat* temp3;
    CvMat* temp4;
    CvMat* temp5;
}
CvKalman;

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

系统运动方程：系统观 测方程：

其 中:

xk(xk − 1) - 系统在时刻 k (k-1) 的状态向量 （state of the system at the moment k (k-1)）zk - 在时刻 k 的系统状态测量向量 （measurement of the system state at the moment k）uk - 应用于时刻 k 的外部控制 (external control applied at the moment k)wk 和 vk 分别为正态分布的运 动和测量噪声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 滤波器结构

CvKalman* cvCreateKalman( int dynam_params, int measure_params, int control_params=0 );

dynam_params状态向量 维数measure_params测量向 量维数control_params控制向 量维数

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

ReleaseKalman

释 放 Kalman 滤波器结构

void cvReleaseKalman( CvKalman** kalman );

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

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

KalmanPredict

估计后来的模型状态

const CvMat* cvKalmanPredict( CvKalman* kalman, const CvMat* control=NULL );
#define cvKalmanUpdateByTime cvKalmanPredict

kalmanKalman 滤波器状态control控制向 量 (uk), 如果没有外部控制 (control_params=0) 应该为 NULL

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

,

其 中

x'k 是预测状态 (kalman->state_pre),xk − 1 是前一步的矫正状态 (kalman->state_post)，应该在开始的某个地方初始化，即缺省为零向量，uk 是外部控制 (control 参数),P'k 是 先验误差相关矩阵 (kalman->error_cov_pre)Pk − 1 是前一步的后验误差相关矩阵 (kalman->error_cov_post),应该在开始的某个地方初始化，即缺省为单位矩阵.

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

KalmanCorrect

调节模型状态

const CvMat* cvKalmanCorrect( CvKalman* kalman, const CvMat* measurement );
#define cvKalmanUpdateByMeasurement cvKalmanCorrect

kalman被更新的 Kalman 结构的指针measurement指向测 量向量的指针，向量形式为 CvMat

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

其 中

zk - 给定测量(mesurement parameter)Kk - 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();/*释放所有窗口*/
}

参 考：opencv中文论坛

 

另外我的程序还实现了图片的打开和保存功能，具 体也是参考了论坛的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);
     }