opencv之卡尔曼滤波

卡尔曼滤波器结构体

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;

以下是转载的卡尔曼滤波器的使用和演示代码

本文的应用是对二维坐标进行预测和平滑

使用方法：

1、初始化

const int stateNum=4;//状态数，包括（x，y，dx，dy）坐标及速度（每次移动的距离）
const int measureNum=2;//观测量，能看到的是坐标值，当然也可以自己计算速度，但没必要
Kalman* kalman = cvCreateKalman( stateNum, measureNum, 0 );//state(x,y,detaX,detaY)

转移矩阵或者说增益矩阵的值好像有点莫名其妙

float A[stateNum][stateNum] ={//transition matrix
		1,0,1,0,
		0,1,0,1,
		0,0,1,0,
		0,0,0,1
	};

看下图就清楚了

X1=X+dx，依次类推

2.预测cvKalmanPredict，然后读出自己需要的值
3.更新观测矩阵
4.更新CvKalman

 只有第一步麻烦些。上述这几步跟代码中的序号对应

#include <cv.h>
#include <cxcore.h>
#include <highgui.h>

#include <cmath>
#include <vector>
#include <iostream>
using namespace std;

const int winHeight=600;
const int winWidth=800;


CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1);

//mouse event callback
void mouseEvent(int event, int x, int y, int flags, void *param )
{
	if (event==CV_EVENT_MOUSEMOVE) {
		mousePosition=cvPoint(x,y);
	}
}

int main (void)
{
 
	//1.kalman filter setup
	const int stateNum=4;
	const int measureNum=2;
	CvKalman* kalman = cvCreateKalman( stateNum, measureNum, 0 );//state(x,y,detaX,detaY)
	CvMat* process_noise = cvCreateMat( stateNum, 1, CV_32FC1 );
	CvMat* measurement = cvCreateMat( measureNum, 1, CV_32FC1 );//measurement(x,y)
	CvRNG rng = cvRNG(-1);
	float A[stateNum][stateNum] ={//transition matrix
		1,0,1,0,
		0,1,0,1,
		0,0,1,0,
		0,0,0,1
	};

	memcpy( kalman->transition_matrix->data.fl,A,sizeof(A));
	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));
	//initialize post state of kalman filter at random
	cvRandArr(&rng,kalman->state_post,CV_RAND_UNI,cvRealScalar(0),cvRealScalar(winHeight>winWidth?winWidth:winHeight));

	CvFont font;
	cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1);

	cvNamedWindow("kalman");
	cvSetMouseCallback("kalman",mouseEvent);
	IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3);
	while (1){
		//2.kalman prediction
		const CvMat* prediction=cvKalmanPredict(kalman,0);
		CvPoint predict_pt=cvPoint((int)prediction->data.fl[0],(int)prediction->data.fl[1]);

		//3.update measurement
		measurement->data.fl[0]=(float)mousePosition.x;
		measurement->data.fl[1]=(float)mousePosition.y;

		//4.update
		cvKalmanCorrect( kalman, measurement );		

		//draw 
		cvSet(img,cvScalar(255,255,255,0));
		cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green
		cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);//current position with red
		char buf[256];
		sprintf(buf,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);
		cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0));
		sprintf(buf,"current position :(%3d,%3d)",mousePosition.x,mousePosition.y);
		cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0));
		
		cvShowImage("kalman", img);
		int key=cvWaitKey(3);
		if (key==27){//esc   
			break;   
		}
	}      

	cvReleaseImage(&img);
	cvReleaseKalman(&kalman);
	return 0;
}