[ECOS] fprintf

iguzzo iguzzo@ig.com.br
Tue Jul 18 20:46:00 GMT 2006


Hello, I'm a brazilian researcher and have any problems when I try run   a 
file with the line code fprintf. 
On the C platform it runs sucessful, but with EB40A (Atmel), run in eCos, it 
failed. 

That is the program: 



#include <stdio.h> 
#include <stdlib.h> 
#include <time.h> 
#include <math.h> 

#define		pi		  3.14159 
const double	dt		= 1.0 / 30.0;	/* 30 Hz sensor update */ 
const double	omega		= 0.5;		/* Max roll rate */ 
const double	Q		= 0.0001;	/* Noise weighting matrix */ 
const double	max_angle	= 30*pi/180.0;	/* Maximum pitch angle */ 
static double	theta		= 4*pi/180;	/* Our initial state estimate */ 
static double	R		= 1;		/* Measurement error weight */ 
static double	P		= 1;		/* Covariance matrix */ 


double 
kalman( 
	double			t,		/* Time */ 
	double			q,		/* Pitching gyro */ 
	double			ax,		/* X acceleration */ 
	double			ay		/* Y acceleration */ 
) 
{ 
	double			Pdot;		/* Derivative of P */ 
	double			E;		/* ? */ 
	double			K;		/* ? */ 

	double			theta_m;	/* Our state measurement */ 

						/* A = 0 */ 
	Pdot = Q;				/* Pdot = A*P + P*A' + Q */ 
	P += Pdot * dt; 

	/* Update our state estimate from the rate gyro */ 
	theta += q * dt; 

	/* We only run the Kalman filter at a slower 10 Hz */ 
	if( (int)( t * 100 ) % 10 != 0 ) 
		return theta; 

	/* Compute our measured state from the accelerometers */ 
	theta_m = atan2( -ay, ax ); 

	E = P + R;				/* E = CPC' + R */ 
	K = P / E;				/* K = PC'inv(E) */ 

	/* Update the state */ 
	theta += K * ( theta_m - theta ); 

	/* Covariance update */ 
	P *= ( 1 - K ); 
	 
	return theta; 
} 

/* 
static inline double 
noise( void ) 
{ 
	return 2.0 * rand_r(time(0)) - 1.0; 
} 
*/ 


int main( void ) 
{ 
	double			t; 
	FILE *fp; 
	 
//	srand( time(0) ); 

	for( t=0 ; t<1.0 ; t+=dt ) 
	{ 
		/* 
		 * Compute our actual state as a function of time. 
		 */ 
		double real_q		= max_angle * omega * cos(omega*t); 
		double real_theta	= max_angle * sin(omega*t); 

		/* 
		 * Fake our sensor readings by adding a little bit of noise 
		 * to the system. 
		 */ 
		double ax =  cos( real_theta );// + noise() * 0.9; 
		double ay = -sin( real_theta );// + noise() * 0.9; 
		double q  = real_q;// + noise() * 8 * pi / 180.0; 

		/* 
		 * Compute our new estimated state with the Kalman filter 
		 */ 
		double theta = kalman( t, q, ax, ay ); 

		fp = fopen("teste.txt","a"); 
		fprintf(fp,"%f %f %f\n", 
			real_theta, 
			theta, 
			real_theta - theta 
		); 
		fclose(fp); 
	} 
return 0; 
} 



Someone can help me? 
Tks 



-------------- next part --------------
-- 
Before posting, please read the FAQ: http://ecos.sourceware.org/fom/ecos
and search the list archive: http://ecos.sourceware.org/ml/ecos-discuss


More information about the Ecos-discuss mailing list