kalman_c.txt

kalman C程序

/*
 *  sparkfun_imu_ahrs.c
 *  
 *
 *  Created by Filipe Varela on 06/04/20.
 *  Copyright 2006 Filipe Varela. All rights reserved.
 *
 *	All code custom writen except:
 *		Kalman algorythm for state_update and kalman_update from rotomotion's tilt.c, credit included in file
 *		Cross axis interference processing matrix from "The Global Positioning System and Inertial Navigation" ISBN: XXX XXXXXXX
 *
 *	As a sidenote, accel2angle scales ADC to G's for future convenience. It is not required for the current status of the project
 *	because trig functions only need equally scaled values. The units are irrelevant.
 *	
 */

#define a2v 00.0048828125
#define r2d 57.29577951308
#define d2r 00.01745329252

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

//socket to broadcast attitude data to another application
struct sockaddr_in sockaddress;
int my_socket;

typedef struct{
	float				time;
	long				satelites;
	float				latitude;
	float				longitude;
	float				altitude;
} gpsPacketStruct;

unsigned char			ultrasound_raw;
float					ultrasound;

unsigned char			compass_low,compass_high;
float					compass_heading;

gpsPacketStruct			gpsPacket;

//state variables
float corrected_rates[3]; //p,q,r in rad/sec
float accels[3]; //Ax,Ay,Az
float gyro_25v_correction[3]; //correction factor for null offset

//kalman estimates
float angle_reference_from_accels[3] = {0.0,0.0,0.0}; //accelerometer based angle estimate
float old_angle_reference_from_accels[3] = {0.0,0.0,0.0}; //previous accelerometer based angle estimate
float angle_estimate[3]; //pretty obvious (current state est)
float gyro_bias[3]={0.0,0.0,0.0};

//raw adc temperature and null temperature
float raw_gyro_temperatures[3]; //roll,pitch,yaw
float raw_gyro_temperatures_null[3] = {524.0,526.0,526.0}; //null values at 25deg (nominal)

//raw adc rate and null rate
float raw_gyro_rates[3]; //roll,pitch,yaw
float raw_gyro_rates_null[3] = {480.0,512.0,496.0}; //rateout at idle (not spinning)

//raw adc gyro 2.5volt reference outputs (for tension (voltage) variation estimate)
float raw_gyro_25v[3]; //roll, pitch, yaw

//raw adc accelerometers
float raw_accels[3]; //x,y,z
float raw_accels_null[3] = {504,504.0,504.0}; //x,y,z at rest and flat

//battery status
float battery;

//gps
float gps_heading;

//period estimation (POSIX time.h) change when resident in atmega
struct timeval start,end;
double dt = 0;

//unix serial port
FILE *fp;

//
float P[3][2][2] = {
	{
		{ 1, 0 },
		{ 0, 1 },
	},
	{
		{ 1, 0 },
		{ 0, 1 },
	},
	{
		{ 1, 0 },
		{ 0, 1 },
	}
};

//accelerometer measurement covariance noise
const float R_angle = 0.3;
const float Q_angle = 0.001;
const float Q_gyro = 0.003;

//accelerometer bias estimate (updated in kalman_update)
float gyro_bias[3];

//prototypes
int readimu(void);
int checkvars(void);
void compensate_gyro_temp_drifts(void);

void accel2angle(void);
void gyro2angle(void);

void state_update();
void kalman_update(int axis_index);

double timeval_subtract(struct timeval *x, struct timeval *y);

/******************************************************************************
	MAIN LOOP
	BROADCASTS UDP FOR THE COCOA HUD/ARTIFICIAL HORIZON (float phi,theta,psi)
*******************************************************************************/
int main(int argc, char *argv[]){
	char		startstop[2] = {'A','Z'};
	int			i;
	
	//inicializar rotinas internas (uarts, timers, etc)
	fp = fopen("/dev/cu.USB Serial","r");
	if(!fp){
		printf("Could not open serial port\n");
		return 0;
	}
	
	//clean possible buffer contents
	for(i=0;i<100;i++)
		readimu();
	
	my_socket = socket(AF_INET, SOCK_DGRAM, 0);
	sockaddress.sin_family = AF_INET;
	sockaddress.sin_port = htons(9999); //host to network short int
	sockaddress.sin_addr.s_addr = INADDR_BROADCAST;
	int yes=1;
	setsockopt(my_socket, SOL_SOCKET, SO_BROADCAST, (char *)&yes, sizeof(yes));
	
	//loop principal
	while(1){
		gettimeofday(&start,0);
		if(readimu()){
			gettimeofday(&end,0);
			dt = timeval_subtract(&end,&start);
			compensate_gyro_temp_drifts();
			gyro2angle();
			state_update();
			accel2angle();
			//check if this accel value is different from last
			
			for(i=0;i<3;i++){
				//got new accel angle estimate?
				//if(old_angle_reference_from_accels[i] != angle_reference_from_accels[i]){
					//update kalman gain
					kalman_update(i);
				}
				//regardless, update old value to this value	
				//old_angle_reference_from_accels[i] = angle_reference_from_accels[i];
			}
			
			//put roll pitch yaw on the wire
			sendto(my_socket, &startstop[0], 1, 0, (struct sockaddr *)&sockaddress, sizeof(sockaddress));
			sendto(my_socket, &angle_estimate, sizeof(angle_estimate), 0, (struct sockaddr *)&sockaddress, sizeof(sockaddress));
			sendto(my_socket, &startstop[1], 1, 0, (struct sockaddr *)&sockaddress, sizeof(sockaddress));
			
		}
		else
			printf("Read imu returned false\n");
	}

	close(my_socket);

	return 0;
}

/******************************************************************************
	ESTIMATE ATTITUDE WITH A KF. NOT REALLY IMPLEMENTED YET, BUT THE
	NAME SUGGESTS WHAT THE INTENTION IS.
*******************************************************************************/
void gyro2angle(void){
	int i;
	float gyro_components[3]; // dot values from matrix
	float sin_roll,tan_pitch, cos_roll, cos_pitch;
	
	//values were in euler. trig could never work right. try matrix again now
	//that we got our internals 100% radians
	sin_roll = sin(angle_estimate[0]);
	cos_roll = cos(angle_estimate[0]);
	cos_pitch = cos(angle_estimate[1]);
	tan_pitch = tan(angle_estimate[1]);
	
	/***************************************************************************
		ESTIMATOR MATRIX
		---------------------------------------
		[phi_dot	]	[1	   sin(phi)tan(theta)	cos(phi)*tan(theta)	]	[P]
		[theta_dot	] = [0		cos(phi)				  -sin(phi)	]	] *	[Q]
		[psi_dot	]	[0	sin(phi)/cos(theta)		cos(phi)/cos(theta)	]	[R]
	***************************************************************************/
	
	//angle-estimate from matrix
	gyro_components[0] = corrected_rates[0]*1 + corrected_rates[1]*sin_roll*tan_pitch + corrected_rates[2]*cos_roll*tan_pitch;
	gyro_components[1] = corrected_rates[0]*0 + corrected_rates[1]*cos_roll + corrected_rates[2]*-sin_roll;
	gyro_components[2] = corrected_rates[0]*0 + corrected_rates[1]*(sin_roll/cos_pitch) + corrected_rates[2]*(cos_roll*cos_pitch);

	for(i=0;i<3;i++){
		//state estimate using integration (rate*time = angle)
		// the filter seems to take a while to converge to correct values after fast jitters
		// i think i may have forgotten to remove the estimate error value here
		angle_estimate[i] += ((dt * gyro_components[i]) - gyro_bias[i]);
	}
	
	printf("Angle estimate (INTERNAL): %f %f %f\n", angle_estimate[0], angle_estimate[1], angle_estimate[2]);
		
	printf("Roll: %4.8f Pitch: %4.8f Yaw: %4.8f\n",
		raw_gyro_rates[0],
		raw_gyro_rates[1],
		raw_gyro_rates[2]);
	printf("Corrected Roll: %4.8f Pitch: %4.8f Yaw: %4.8f\n",
		corrected_rates[0],
		corrected_rates[1],
		corrected_rates[2]);
	printf("PTEMP: %4.8f QTEMP: %4.8f RTEMP: %4.8f\n",
		raw_gyro_temperatures[0],
		raw_gyro_temperatures[1],
		raw_gyro_temperatures[1]);
	printf("Accel x: %4.8f (%4.8f volt) y: %4.8f (%4.8f volt) z: %4.8f (%4.8f volt)\n",
		raw_accels[0], raw_accels[0]*a2v,
		raw_accels[1], raw_accels[1]*a2v,
		raw_accels[2], raw_accels[2]*a2v);
	printf("2.5v References-Roll: %4.8f Pitch: %4.8f Yaw: %4.8f\n",
		raw_gyro_25v[0],
		raw_gyro_25v[1],