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]):

系统运动方程： $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博客园博客，原文链接：http://www.cnblogs.com/feisky/archive/2009/12/04/1617287.html，如需转载请自行联系原作者

OpenCV 卡尔曼滤波器的使用

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

CvKalman

CreateKalman

ReleaseKalman

KalmanPredict

KalmanCorrect

热门文章

最新文章

相关电子书

探索云世界

热门

云计算

大数据

云原生

人工智能

数据库

开发与运维

活动广场

任务中心

训练营

直播

乘风者计划

下载

镜像站

技术资料

OpenCV 卡尔曼滤波器的使用

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

CvKalman

CreateKalman

ReleaseKalman

KalmanPredict

KalmanCorrect

热门文章

最新文章

相关电子书