1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
|
#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;
} |
