OpenCV 卡尔曼滤波器的使用

Posted on 2009-12-04 22:12 feisky 阅读(17386) 评论(5) 编辑收藏举报

首先来看一下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]):

系统运动方程： $x_k=A\cdot x_{k-1}+ B\cdot u_k+w_k$

系统观测方程： $z_k=H\cdot x_k + v_k$

其中:

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 滤波器结构

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=A \cdot x_{k-1}+ B \cdot u_k$

$P'_k=A \cdot P_{k-1} \cdot A^T + Q$ ,

其中

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

调节模型状态

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

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

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

$K_k=P'_k\cdot H^T \cdot (H \cdot P'_k \cdot H^T+R)^{-1}$

$x_k=x'_k+K_k \cdot (z_k-H \cdot x'_k)$

$P_k=(I-K_k \cdot H)\cdot P'_k$

其中

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

参考：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);
	}

刷新页面返回顶部

feisky

公告