{

double t = dis ( gen );

fprintf ( stderr, "t=%f\n", t );

return t;

{

( void ) argc;

( void ) argv;

float x_last = 0;

float p_last = 0.02;

float Q = 0.018;

float R = 0.542;

float kg;

float karman_mid;

float kalman_measure;

float p_mid;

float p_now;

float z_real = 0.56;

float z_measure;

float summerror_kalman = 0;

float summerror_measure = 0;

int i;

x_last = z_real + frand();

karman_mid = x_last;

for ( i = 0; i < 3000; i++ )

{

karman_mid = x_last;

p_mid = p_last + Q;

kg = p_mid / ( p_mid + R );

z_measure = z_real + frand(); //测量值

kalman_measure = karman_mid + kg * ( z_measure - karman_mid ); //估计出的最有值

p_now = ( 1 - kg ) * p_mid; //最优值对应的协方差

#if 1

printf ( "Real position:%6.3f\n", z_real );

printf ( "Measure position:%6.3f [diff:%.3f]\n", z_measure, fabs ( z_real - z_measure ) );

printf ( "Kalman position: %6.3f [diff:%.3f]\n", kalman_measure, fabs ( z_real - kalman_measure ) );

printf ( "\n\n" );

#endif

summerror_measure += fabs ( z_real - z_measure );

summerror_kalman += fabs ( z_real - kalman_measure );

p_last = p_now;

x_last = kalman_measure;

}

printf ( "total error :%f\n", summerror_measure );

printf ( "total kalman error :%f\n", summerror_kalman );

printf ( "pesent kalman error:%d%%\n", ( int ) ( ( summerror_kalman / summerror_measure ) * 100 ) );