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