UKHAS Wiki

UK High Altitude Society

User Tools

Site Tools


code:fit

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision
Previous revision
code:fit [2010/01/23 12:04] laurencebcode:fit [2011/05/14 15:24] (current) – neater code laurenceb
Line 1: Line 1:
 ====== Gnu Octave calibration routine ====== ====== 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. +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.
 <code matlab> <code matlab>
-function bias_gain=nr_fit(+function bias_gain=nr_fit(data,initial) %data is in x,y,z format with samples along columns 
-fp=fopen('rpy90bothways.TXT','r')+ datalenght=size(data)(2) 
- + %bias_gain=[0.0015;0.0015;0.0015;0;0;0]; - e.g. for lsm303dlh magno (this has to be close to ~order of magnitude) 
-data=fscanf(fp,'%f    %f    %f    %f    %f    %f    %f    %f    %f    %f',[10,inf]); + bias_gain=initial
-accel=data(5:7,:); + convergence=0.5;iterations=0; 
-datalenght=size(data)(2); + while(convergence<0.99 || convergence>1) 
-bias_gain=[1;1;1;0;0;0]; + measured=[bias_gain(1),0,0;0,bias_gain(2),0;0,0,bias_gain(3)]*(data(1:3,:)-repmat(bias_gain(4:6),1,datalenght)); 
-err_mean=1+ errors=1.-sum(measured.*measured); %try to normalise the vectors 
-correction=zeros(6,1); + corrections=[2*bias_gain(1)*(data(1,:)-bias_gain(4)).^2;2*bias_gain(2)*(data(2,:)-bias_gain(5)).^2;... 
-while(err_mean>0.02) + 2*bias_gain(3)*(data(3,:)-bias_gain(6)).^2;-2*(bias_gain(1)^2)*(data(1,:)-bias_gain(4));... 
- err_mean + -2*(bias_gain(2)^2)*(data(2,:)-bias_gain(5));-2*(bias_gain(3)^2)*(data(3,:)-bias_gain(6))]; 
- fflush(stdout); + bias_gain+=pinv(corrections')*errors'./2; %note the factor of 1/2 - this increases stability 
- err_mean=0; + err_mean=sqrt(mean(abs(errors))+1)-1
- correction_total=zeros(6,1); + if(iterations==1) 
- for(i=1:datalenght+ convergence=(err_mean/old_err_mean); %we iterate until we stop converging 
- measured=[bias_gain(1),0,0;0,bias_gain(2),0;0,0,bias_gain(3)]*(accel(1:3,i)-bias_gain(4:6)); + elseif(iterations>1) 
- erro=1-dot(measured,measured); + convergence=0.66*convergence+0.34*(err_mean/old_err_mean); 
- 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))]; + endif 
- correction*=erro/(datalenght*dot(correction,correction))+ old_err_mean=err_mean
- err_mean+=abs(erro)/datalenght+ iterations++; 
- correction_total+=correction; + printf("Convergence: %f, Residual: %f\n",convergence,err_mean); 
- endfor + fflush(stdout); 
- bias_gain+=correction_total; + endwhile
-endwhile +
-bias_gain(1)=abs(1/bias_gain(1)); +
-bias_gain(2)=abs(1/bias_gain(2)); +
-bias_gain(3)=abs(1/bias_gain(3)); +
-bias_gain(4)*=-1+
-bias_gain(5)*=-1+
-bias_gain(6)*=-1; +
 endfunction  endfunction
 </code> </code>
  
code/fit.1264248298.txt.gz · Last modified: 2010/01/23 12:04 by laurenceb

Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki