code:fit
Differences
This shows you the differences between two versions of the page.
Next revision | Previous revision | ||
code:fit [2010/01/23 12:04] – created laurenceb | code: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(' | + | datalenght=size(data)(2) |
- | + | %bias_gain=[0.0015;0.0015;0.0015; | |
- | data=fscanf(fp,' | + | bias_gain=initial; |
- | accel=data(5: | + | convergence=0.5;iterations=0; |
- | datalenght=size(data)(2); | + | while(convergence< |
- | bias_gain=[1;1;1;0;0;0]; | + | measured=[bias_gain(1), |
- | err_mean=1; | + | errors=1.-sum(measured.*measured); %try to normalise the vectors |
- | correction=zeros(6, | + | corrections=[2*bias_gain(1)*(data(1,:)-bias_gain(4)).^2; |
- | while(err_mean> | + | 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)); |
- | fflush(stdout); | + | bias_gain+=pinv(corrections' |
- | err_mean=0; | + | err_mean=sqrt(mean(abs(errors))+1)-1; |
- | correction_total=zeros(6,1); | + | if(iterations==1) |
- | for(i=1: | + | convergence=(err_mean/old_err_mean); %we iterate until we stop converging |
- | measured=[bias_gain(1), | + | elseif(iterations> |
- | 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; | + | endif |
- | correction*=erro/(datalenght*dot(correction, | + | old_err_mean=err_mean; |
- | err_mean+=abs(erro)/datalenght; | + | iterations++; |
- | correction_total+=correction; | + | printf(" |
- | endfor | + | fflush(stdout); |
- | bias_gain+=correction_total; | + | endwhile |
- | endwhile | + | |
- | bias_gain(1)=abs(1/bias_gain(1)); | + | |
- | bias_gain(2)=abs(1/ | + | |
- | bias_gain(3)=abs(1/bias_gain(3)); | + | |
- | bias_gain(4)*=-1; | + | |
- | bias_gain(5)*=-1; | + | |
- | bias_gain(6)*=-1; | + | |
endfunction | endfunction | ||
</ | </ | ||
code/fit.1264248290.txt.gz · Last modified: 2010/01/23 12:04 by laurenceb