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
- kalman
- Kalman 滤波器状态
- 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);
}
