2 ## Tracking of rotating point.
3 ## Rotation speed is constant.
4 ## Both state and measurements vectors are 1D (a point angle),
5 ## Measurement is the real point angle + gaussian noise.
6 ## The real and the estimated points are connected with yellow line segment,
7 ## the real and the measured points are connected with red line segment.
8 ## (if Kalman filter works correctly,
9 ## the yellow segment should be shorter than the red one).
10 ## Pressing any key (except ESC) will reset the tracking with a different speed.
11 ## Pressing ESC will stop the program.
18 function ret=calc_point(angle)
20 ret=cvPoint( cvRound(img.width/2 + img.width/3*cos(angle)), \
21 cvRound(img.height/2 - img.width/3*sin(angle)));
24 function draw_cross( center, color, d )
27 cvLine( img, cvPoint( center.x - d, center.y - d ),
28 cvPoint( center.x + d, center.y + d ), color, 1, CV_AA, 0);
29 cvLine( img, cvPoint( center.x + d, center.y - d ),
30 cvPoint( center.x - d, center.y + d ), \
36 img = cvCreateImage( cvSize(500,500), 8, 3 );
37 kalman = cvCreateKalman( 2, 1, 0 );
38 state = cvCreateMat( 2, 1, CV_32FC1 ); # (phi, delta_phi)
39 process_noise = cvCreateMat( 2, 1, CV_32FC1 );
40 measurement = cvCreateMat( 1, 1, CV_32FC1 );
44 cvZero( measurement );
45 cvNamedWindow( "Kalman", 1 );
48 cvRandArr( rng, state, CV_RAND_NORMAL, cvRealScalar(0), cvRealScalar(0.1) );
50 kalman.transition_matrix = mat2cv(A, CV_32FC1);
51 cvSetIdentity( kalman.measurement_matrix, cvRealScalar(1) );
52 cvSetIdentity( kalman.process_noise_cov, cvRealScalar(1e-5) );
53 cvSetIdentity( kalman.measurement_noise_cov, cvRealScalar(1e-1) );
54 cvSetIdentity( kalman.error_cov_post, cvRealScalar(1));
55 cvRandArr( rng, kalman.state_post, CV_RAND_NORMAL, cvRealScalar(0), cvRealScalar(0.1) );
59 state_angle = state(0);
60 state_pt = calc_point(state_angle);
62 prediction = cvKalmanPredict( kalman );
63 predict_angle = prediction(0);
64 predict_pt = calc_point(predict_angle);
66 cvRandArr( rng, measurement, CV_RAND_NORMAL, cvRealScalar(0), \
67 cvRealScalar(sqrt(kalman.measurement_noise_cov(0))) );
69 ## generate measurement
70 cvMatMulAdd( kalman.measurement_matrix, state, measurement, measurement );
72 measurement_angle = measurement(0);
73 measurement_pt = calc_point(measurement_angle);
77 draw_cross( state_pt, CV_RGB(255,255,255), 3 );
78 draw_cross( measurement_pt, CV_RGB(255,0,0), 3 );
79 draw_cross( predict_pt, CV_RGB(0,255,0), 3 );
80 cvLine( img, state_pt, measurement_pt, CV_RGB(255,0,0), 3, CV_AA, 0 );
81 cvLine( img, state_pt, predict_pt, CV_RGB(255,255,0), 3, CV_AA, 0 );
83 cvKalmanCorrect( kalman, measurement );
85 cvRandArr( rng, process_noise, CV_RAND_NORMAL, cvRealScalar(0), \
86 cvRealScalar(sqrt(kalman.process_noise_cov(0)(0))));
87 cvMatMulAdd( kalman.transition_matrix, state, process_noise, state );
89 cvShowImage( "Kalman", img );
90 code = cvWaitKey( 100 );
97 if( code == '\x1b' || code == 'q' || code == 'Q' )
102 cvDestroyWindow("Kalman");