UKHAS Wiki

UK High Altitude Society

User Tools

Site Tools


code:fit

This is an old revision of the document!


Gnu Octave calibration routine

This is a spheroid fitter for obtaining sensor bias and gain for each axis of an accelerometer or magnetometer given a set of sample data. bias_gain needs to be initialized to a reasonably sensible value for this to be well behaved.

function bias_gain=nr_fit_()
fp=fopen('speedevilaccel.txt','r');
data=fscanf(fp,'%f    %f    %f    %f    %f',[5,inf]);
size(data);
accel=data(3:5,:);
datalenght=size(data)(2);
bias_gain=[0.001;0.001;0.001;0;0;0];
convergence=0.5;iterations=0;
correction=zeros(6,1);
while(convergence<0.95 || convergence>1)
	err_mean=0;
	correction_total=zeros(6,1);
	for(i=1:datalenght)
		measured=[bias_gain(1),0,0;0,bias_gain(2),0;0,0,bias_gain(3)]*(accel(1:3,i)-bias_gain(4:6));
		erro=1-dot(measured,measured);
		correction=[2*bias_gain(1)*(accel(1,i)-bias_gain(4))^2,2*bias_gain(2)*(accel(2,i)-bias_gain(5))^2,2*bias_gain(3)*(accel(3,i)-bias_gain(6))^2,-2*(bias_gain(1)^2)*(accel(1,i)-bias_gain(4)),-2*(bias_gain(2)^2)*(accel(2,i)-bias_gain(5)),-2*(bias_gain(3)^2)*(accel(3,i)-bias_gain(6))];
		if(i==1)
			c_matrix=correction;
			errors=erro;		
		else
			c_matrix=[c_matrix;correction];
			errors=[errors;erro];
		endif
	endfor
	bias_gain+=pinv(c_matrix)*errors;
	err_mean=mean(abs(errors));
	if(iterations==1)
		convergence=(err_mean/old_err_mean);
	elseif(iterations>1)
		convergence=0.66*convergence+0.34*(err_mean/old_err_mean);
	endif
	old_err_mean=err_mean;
	iterations++;
	printf("Converg: %f, Residual: %f\n",convergence,err_mean);
	fflush(stdout);
endwhile	
endfunction	
code/fit.1264634412.txt.gz · Last modified: 2010/01/27 23:20 by laurenceb

Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki