<span style="font-size:14px;">#include <opencv\cv.h>#include <opencv\highgui.h>#include <math.h>int main(int argc, char** argv){const float A[] = { 1, 1, 0, 1 };IplImage* img = cvCreateImage( cvSize(500,500), 8, 3 );CvKalman* kalman = cvCreateKalman( 2, 1, 0 );// 状态是角度和角度增量CvMat* state = cvCreateMat( 2, 1, CV_32FC1 );CvMat* process_noise = cvCreateMat( 2, 1, CV_32FC1 );// 只有角度被测量CvMat* measurement = cvCreateMat( 1, 1, CV_32FC1 );CvRandState rng;int code = -1;cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );cvZero( measurement );cvNamedWindow( "Kalman", 1 );for(;;){cvRandSetRange( &rng, 0, 0.1, 0 );rng.disttype = CV_RAND_NORMAL;cvRand( &rng, state );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));// 选择随机的开始状态cvRand( &rng, kalman->state_post );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);// 预测点的方向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 );cvMatMulAdd( kalman->measurement_matrix,state, measurement, measurement );measurement_angle = measurement->data.fl[0];measurement_pt = calc_point(measurement_angle);#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 );// 调整Kalman滤波器的状态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( 100 );if( code > 0 )break;}if( code == 27 )break;}return 0;}</span>
运行结果截图：
，人生就像是一场旅行，遇到的既有感人的，
