基于OpenCV的Kalman滤波器

#include "cv.h" #include "highgui.h" #include <math.h> int main(int argc, char** argv) { /* A matrix data */ 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 ); CvMat* process_noise = cvCreateMat( 2, 1, CV_32FC1 ); /* only phi (angle) is measured */ 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)); /* choose random initial state */ 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); /* 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( 100 ); if( code > 0 ) /* break current simulation by pressing a key */ break; } if( code == 27 ) /* exit by ESCAPE */ break; } return 0; }