kalman.cpp

kalman滤波

#include "cv.h"
#include "highgui.h"
#include <math.h>
#include "time.h"
#include "stdio.h"
#define QQ 1 //噪声协方差
#define RR 1
#define PP 2 //先验误差相关矩阵
#define TF 256  //模拟长度
#define  PN 100  //粒子数目
int main(int argc, char** argv)
{
    /* A matrix data */
	FILE *pfile;
	int i,j;
	double xtmp,ytmp;
	int src[256];
	double preArr[256];
	double F,P,H,K;
	for (i=0;i<256;i++)
	{
  		src[i]=-1;
		preArr[i]=-1;
	}
	double xhat=0.1;
// 	pfile=fopen("histo1.txt","r");
	pfile=fopen("histbaboongray.txt","r");

	for (i=0;i<256;i++)
	{
		fscanf(pfile,"%d ",&src[i]);
	}
	fclose(pfile);
	for (i=0;i<256;i++)
	{
		printf("%d  \n",src[i]);
	}


	for (i=0;i<TF-1;i++)
	{
// 		srand( (unsigned)time( NULL ) );
		int a=rand()%10000;
		double b=(double)a/10000.0;
		xtmp=src[i]+sqrt(QQ)*b;

		a=rand()%10000;
		b=(double)a/10000.0;
		ytmp=pow(xtmp,2)/20+sqrt(RR)*b;

		F=0.5+25*(1-pow(xhat,2))/pow(1+pow(xhat,2),2);
		P=F*P*F+QQ;
		H=xhat/10;
		K=P*H/(H*P*H+RR);
		xhat=0.5*xhat+25*xhat/(1+pow(xhat,2));
		xhat=xhat+K*(ytmp-pow(xhat,2)/20);
		P=(1-K*H)*P;
		preArr[i]=xhat;
	}

	for (i=0;i<256;i++)
	{
		printf("%d  %.2f \n",src[i],preArr[i]);
	}
	pfile=fopen("saved.txt","w");
	for (i=0;i<256;i++)
	{
		fprintf(pfile,"%d  %.2f\n",src[i],preArr[i]);
	}
	fclose(pfile);
    return 0;
}
// /*	pfile=fopen("histo1.txt","wr");
//     const float A[] = { 1, 1, 0, 1 };
// 
//     IplImage* img = cvCreateImage( cvSize(500,500), 8, 3 );
//     CvKalman* kalman = cvCreateKalman( 2, 1, 0 );//状态向量维数，观测向量维数，控制向量维数
//     /* state is (phi, delta_phi) - angle and angle increment */
//     CvMat* state = cvCreateMat( 2, 1, CV_32FC1 );//2行1列，32位浮点型
//     CvMat* process_noise = cvCreateMat( 2, 1, CV_32FC1 );
//     /* only phi (angle) is measured */
//     CvMat* measurement = cvCreateMat( 1, 1, CV_32FC1 );
//     
// 	CvRandState rng; //产生随机向量的结构数组： Array of structures to generate random vectors
// 	//初始化随机数生成器状态；
//     int code = -1;
// 
//     cvRandInit( &rng, 0, 2, -2, CV_RAND_UNI );//均匀分布随机矩阵，虚参数，之后再调整
// 
//     cvZero( measurement );
//     cvNamedWindow( "Kalman", 1 );
// 
//     for(;;)
//     {
//         cvRandSetRange( &rng, 0, 0.1, 0 );//deviation ,average point brightness, initial all the dimension
//         rng.disttype = CV_RAND_NORMAL;//分布
// 
//         cvRand( &rng, state );
// 
//         memcpy( kalman->transition_matrix->data.fl, A, sizeof(A));//状态转移矩阵，拷贝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));//误差相关系数矩阵
//         /*
// 
// 			  SetIdentity
// 			初始化带尺度的单位矩阵 
// 
// 			void cvSetIdentity( CvArr* mat, CvScalar value=cvRealScalar(1) );
// 			mat 
// 			待初始化的矩阵 (不一定是方阵)。 
// 			value 
// 			赋值给对角线元素的值。 
// 			函数 cvSetIdentity 初始化带尺度的单位矩阵: 
// 
// 			arr(i,j)=value 如果 i=j, 
// 			否则为 0 
// 
// 		*/	
// 		/* choose random initial state */
//         cvRand( &rng, kalman->state_post );//KALMAN状态阵
// 
//         rng.disttype = CV_RAND_NORMAL;
// 
//         for(;;)
//         {
//             #define calc_point(angle)                                      \
//                 cvPoint( cvRound(img->width/2 + img->width/3*cos(angle)),  \
//                          cvRound(img->height/2 - img->width/3*sin(angle)))
// 
//             float state_angle = state->data.fl[0];
//             CvPoint state_pt = calc_point(state_angle);
// 
//             /* predict point position */
//             const CvMat* prediction = cvKalmanPredict( kalman, 0 );
//             float predict_angle = prediction->data.fl[0];
//             CvPoint predict_pt = calc_point(predict_angle);
//             float measurement_angle;
//             CvPoint measurement_pt;
// 
// 			cvRandSetRange( &rng, 0, 
// 							sqrt(kalman->measurement_noise_cov->data.fl[0]), 0 );
// 							cvRand( &rng, measurement );
// 
//             /* generate measurement */
// 			cvMatMulAdd( kalman->measurement_matrix, 
// 						state, measurement, measurement );
// 
//             measurement_angle = measurement->data.fl[0];
//             measurement_pt = calc_point(measurement_angle);
// 
//             /* plot points */
//             #define draw_cross( center, color, d )                                 \
//                 cvLine( img, cvPoint( center.x - d, center.y - d ),                \
//                              cvPoint( center.x + d, center.y + d ), color, 1, 0 ); \
//                 cvLine( img, cvPoint( center.x + d, center.y - d ),                \
//                              cvPoint( center.x - d, center.y + d ), color, 1, 0 )
// 
//             cvZero( img );
//             draw_cross( state_pt, CV_RGB(255,255,255), 3 );
//             draw_cross( measurement_pt, CV_RGB(255,0,0), 3 );
//             draw_cross( predict_pt, CV_RGB(0,255,0), 3 );
//             cvLine( img, state_pt, predict_pt, CV_RGB(255,255,0), 3, 0 );
// 
// //             /* adjust Kalman filter state */
//             cvKalmanCorrect( kalman, measurement );
// 
//             cvRandSetRange( &rng, 0, sqrt(kalman->process_noise_cov->data.fl[0]), 0 );
//             cvRand( &rng, process_noise );
//             cvMatMulAdd( kalman->transition_matrix, state, process_noise, state );
// 
//             cvShowImage( "Kalman", img );
//             code = cvWaitKey( 0 );
// 
// //             if( code > 0 )
// //                 break;
//         }
//         if( code == 27 ) 
//             break;
//     }