kalman_c.txt

kalman C程序

		raw_gyro_25v[2]);
	printf("Time ref: %4.8lf Sample freq: %4.8lf\n", dt, 1/dt);
	printf("Accelerometer state est: Roll: %4.8f Pitch: %4.8f Yaw: %4.8f\n",
		angle_reference_from_accels[0],
		angle_reference_from_accels[1],
		angle_reference_from_accels[2]);
	printf("Kalman filtered output: Roll: %4.8f Pitch: %4.8f Yaw: %4.8f\n",
		angle_estimate[0],
		angle_estimate[1],
		angle_estimate[2]);
	printf("Kalman filtered output degs: Roll: %4.8f Pitch: %4.8f Yaw: %4.8f\n",
		angle_estimate[0]*r2d,
		angle_estimate[1]*r2d,
		angle_estimate[2]*r2d);
	printf("Gps size: %d time: %f satellites: %d lat: %f lon: %f, alt: %f\n",
		sizeof(gpsPacket),
		gpsPacket.time,
		gpsPacket.satelites,
		gpsPacket.latitude,
		gpsPacket.longitude,
		gpsPacket.altitude);
	printf("Ultrasound altimeter: %f\n", ultrasound);
	printf("Compass heading: %d %d %f\n", compass_low,compass_high,compass_heading);
	printf("Battery: %f\n\n", battery);
}

/*******************************************************************
	SAMPLE SERIAL LINE FOR IMU DATA
	32 byte packet (16 short values)
********************************************************************/
int readimu(void){
	char		c,d;
	int			i;
	short		temp;
	
	fread(&c,1,1,fp);
	if(c=='A'){
		printf("Got start\n");
		fread(&ultrasound_raw,1,1,fp);
		ultrasound = (((ultrasound_raw * 5.0) / 255.0) / 0.01) * 2.24;
		
		fread(&compass_low,1,1,fp);
		fread(&compass_high,1,1,fp);
		
		compass_heading = (compass_high * 256.0) + compass_low;
		
		fread(&gpsPacket.altitude,4,1,fp);
		fread(&gpsPacket.longitude,4,1,fp);
		fread(&gpsPacket.latitude,4,1,fp);
		fread(&gpsPacket.satelites,4,1,fp);
		fread(&gpsPacket.time,4,1,fp);
				
		//got packet start. read next 33 bytes, count samples and check for packet end byte.
		fread(&temp,2,1,fp); raw_gyro_rates[1] = temp;			//gyro rateout
		fread(&temp,2,1,fp); raw_gyro_25v[1] = temp;			//2.5v ref
		fread(&temp,2,1,fp); raw_gyro_temperatures[1]=temp;		//gyro temp
		fread(&temp,2,1,fp); //raw_accels[1] = temp;			//accel y raw (patch y)
		fread(&temp,2,1,fp); raw_accels[2] = temp;				//accel x raw (patch z)

		fread(&temp,2,1,fp); raw_gyro_rates[0] = temp;			//gyro rateout
		fread(&temp,2,1,fp); raw_gyro_25v[0] = temp;			//2.5v ref
		fread(&temp,2,1,fp); raw_gyro_temperatures[0]=temp;		//gyro temp
		fread(&temp,2,1,fp); //raw_accels[0] = temp;			//accel y raw (patch z)
		fread(&temp,2,1,fp); //raw_accels[2] = temp;			//accel x raw

		fread(&temp,2,1,fp); raw_gyro_rates[2] = temp;			//gyro rateout
		fread(&temp,2,1,fp); raw_gyro_25v[2] = temp;			//2.5v ref
		fread(&temp,2,1,fp); raw_gyro_temperatures[2]=temp;		//gyro temp
		fread(&temp,2,1,fp); raw_accels[1] = temp;				//accel y raw (patch y)
		fread(&temp,2,1,fp); raw_accels[0] = temp;				//accel x raw (patch z)
		
		fread(&temp,2,1,fp); battery = temp;					//battery voltage
		
		fread(&c,1,1,fp);
	}
	//appply correction factors (from 2.5v ref) to roll gyro outputs
	for(i=0;i<3;i++){
		gyro_25v_correction[i] = (2.5/raw_gyro_25v[i])*1024;
		gyro_25v_correction[i] = (gyro_25v_correction[i]/5);
		raw_gyro_rates[i] = gyro_25v_correction[i]*raw_gyro_rates[i];
		raw_gyro_temperatures[i] = gyro_25v_correction[i]*raw_gyro_temperatures[i];
	}
		
	if(c=='Z' && checkvars())
		return 1;
	else{
		printf("Errored\n");
		return 0;
	}
}

/***************************************************************************
	3 POINT GYRO TEMPERATURE CALIBRATION BASED ON ANALOG'S APPNOTE.
	NEEDS ADDITIONAL WORK AND WE REALLY SHOULD IGNORE TABLE VALUES AND
	MEASURE EACH OF OUR GYROS AND THEN CREATE A TEMPvsOUTPUT TABLE FOR THEM
***************************************************************************/
void compensate_gyro_temp_drifts(){
	//local variables
	float gyro_null[3],gyro_scale_factor[3];
	float gyro_temp_differential[3];
	float gyro_temp_differential_squared[3];
	float gyro_rates_temp_compensated[3];
	int i;
	
	//if we convert to rads BEFORE the correction, can the extra precision of
	//the float's mantissa increase the scale precision? probably not, but we'll
	//try it later just to be sure.
	
	for(i=0;i<3;i++){
		//calculate null temp point (temp out at current vout)
		//current temp - reference temp (25 degrees celsius)
		gyro_temp_differential[i] = (raw_gyro_temperatures[i]-raw_gyro_temperatures_null[i])*a2v;
		//previous var square
		gyro_temp_differential_squared[i] = gyro_temp_differential[i]*gyro_temp_differential[i];
		//current null value (center) from reference null value
		gyro_null[i] = (raw_gyro_rates_null[i]*a2v) + 0.0086*(gyro_temp_differential[i])+0.03597*(gyro_temp_differential_squared[i]);
		//compensate current output with temp correction
		gyro_rates_temp_compensated[i] = (raw_gyro_rates[i]*a2v) - gyro_null[i];
		//convert to degrees by applying the scale factor according to temp
		gyro_scale_factor[i] = 12.744 + 1.26056*(gyro_temp_differential[i])+0.6728*(gyro_temp_differential_squared[i]);
		//transform P into corrected degrees per second
		corrected_rates[i] = gyro_rates_temp_compensated[i] / (gyro_scale_factor[i]/1000);
		// WE'RE FINISHED EXECUTING OUR CORRECTIONS.
		// WE NOW HAVE EXPORTED CORRECTED VALUES FOR P,Q,R (ROLL,PITCH,YAW) as float corrected_rates in EULER.
		//WE WANT RADS
		corrected_rates[i] = corrected_rates[i]*d2r - gyro_bias[i];
	}
}

/*******************************************************************
	TRANSFORM ACCELEROMETER DATA INTO ANGLES (RADIANS)
********************************************************************/
void accel2angle(void){
	int i;
	// Create reference attitute estimation for matrix.
	
	// when perpendicular to level, output accuracy decreases 17.5mg per angle of tilt, so
	// use last angle to calculate scale factor change
	// real_accel[i] = accels[i] - 0.0175*angle_estimate[i]
	
	accels[0] = ((raw_accels[0] - raw_accels_null[0])*a2v)/0.312;
	accels[1] = ((raw_accels[1] - raw_accels_null[1])*a2v)/0.312;
	accels[2] = ((raw_accels[2] - raw_accels_null[2])*a2v)/0.312; //was 1
	
	// Calculate g ( ||g_vec|| )
	float g = sqrt(accels[0]*accels[0] + accels[1]*accels[1] + accels[2]*accels[2]);
	
	printf("Gs: %4.8f %4.8f %4.8f (g=%f)\n", accels[0], accels[1], accels[2],g);
		
	if(g==0)
		exit(1);
	
	//values in radians
	angle_reference_from_accels[0] = -asin(accels[0] / g);
	angle_reference_from_accels[1] = asin(accels[1] / g);
	angle_reference_from_accels[2] = 0.0;//compass_heading*d2r; //gps_heading or compass;
}

/***********************************************************************
	SANITIZE OUR INPUT PACKETS
************************************************************************/
int checkvars(void){
	int i;
	
	//just a sanity check. if not good, return false so the main iterator can
	//ignore this packet.
	printf("Raw accels: %f %f %f\n", raw_accels[0],raw_accels[1],raw_accels[2]);
	for(i=0;i<3;i++){
		if(raw_gyro_rates[i] <= 0 || raw_gyro_rates[i] >= 1024 || raw_accels[i] <= 0 && raw_accels[i] >= 1024)
		return 0;
	}
	
	return 1;
}

/***********************************************************************
	Microsecond precision difftime for "accurate" integration
************************************************************************/
double timeval_subtract(struct timeval *x, struct timeval *y){
	double a,b,result;
	
	a = x->tv_sec + (x->tv_usec/1000000.0);
	b = y->tv_sec + (y->tv_usec/1000000.0);
	
	result = a-b;
	
	return(result);
}

void state_update() //called on new gyro output available
{
	int i;
	
	//state update really is just a progression of the internal covariance estimate.
	//it does not use any imu output, only the time ref.
	//the integration, unlike what happens with rotomotion's code, is done in gyro2angle.
	//this allows us to track cross-axis interference :)
	
	for(i=0;i<3;i++){
		float Pdot[2 * 2] = {
			Q_angle - P[i][0][1] - P[i][1][0],	/* 0,0 */
					- P[i][1][1],				/* 0,1 */
					- P[i][1][1],				/* 1,0 */
			Q_gyro								/* 1,1 */
		};

		/* Update the covariance matrix */
		P[i][0][0] += Pdot[0] * dt;
		P[i][0][1] += Pdot[1] * dt;
		P[i][1][0] += Pdot[2] * dt;
		P[i][1][1] += Pdot[3] * dt;
	}
}

void kalman_update(int axis_index) // i is axis index
{
	float angle_err;
	
	const float C_0 = 1.0;
	
	angle_err = angle_reference_from_accels[i] - angle_estimate[i];

	const float PCt_0 = C_0 * P[i][0][0];
	const float PCt_1 = C_0 * P[i][1][0];
			
	//error estimate
	const float E = R_angle + C_0 * PCt_0;

	const float	K_0 = PCt_0 / E;
	const float	K_1 = PCt_1 / E;
			
	const float	t_0 = PCt_0;
	const float	t_1 = C_0 * P[i][0][1];

	P[i][0][0] -= K_0 * t_0;
	P[i][0][1] -= K_0 * t_1;
	P[i][1][0] -= K_1 * t_0;
	P[i][1][1] -= K_1 * t_1;
		
	angle_estimate[i] += K_0 * angle_err;
	gyro_bias[i] += K_1 * angle_err;
}