code:fit
Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revision | ||
code:fit [2010/01/27 23:20] – laurenceb | code:fit [2011/05/14 15:24] (current) – neater code laurenceb | ||
---|---|---|---|
Line 3: | Line 3: | ||
bias_gain needs to be initialized to a reasonably sensible value for this to be well behaved. | 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;0;0;0]; - e.g. for lsm303dlh magno (this has to be close to ~order of magnitude) | |
- | data=fscanf(fp,' | + | bias_gain=initial; |
- | size(data); | + | convergence=0.5; |
- | accel=data(3: | + | while(convergence< |
- | datalenght=size(data)(2); | + | measured=[bias_gain(1), |
- | bias_gain=[0.001;0.001;0.001;0;0;0]; | + | errors=1.-sum(measured.*measured); %try to normalise the vectors |
- | convergence=0.5; | + | corrections=[2*bias_gain(1)*(data(1,:)-bias_gain(4)).^2;2*bias_gain(2)*(data(2,:)-bias_gain(5)).^2;... |
- | correction=zeros(6, | + | 2*bias_gain(3)*(data(3,:)-bias_gain(6)).^2;-2*(bias_gain(1)^2)*(data(1,:)-bias_gain(4));... |
- | while(convergence< | + | -2*(bias_gain(2)^2)*(data(2,:)-bias_gain(5));-2*(bias_gain(3)^2)*(data(3,:)-bias_gain(6))]; |
- | err_mean=0; | + | bias_gain+=pinv(corrections' |
- | correction_total=zeros(6, | + | err_mean=sqrt(mean(abs(errors))+1)-1; |
- | for(i=1: | + | if(iterations==1) |
- | measured=[bias_gain(1), | + | convergence=(err_mean/ |
- | erro=1-dot(measured,measured); | + | elseif(iterations> |
- | 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))]; | + | convergence=0.66*convergence+0.34*(err_mean/ |
- | if(i==1) | + | |
- | c_matrix=correction; | + | |
- | errors=erro; | + | |
- | else | + | |
- | c_matrix=[c_matrix; | + | |
- | errors=[errors; | + | |
endif | endif | ||
- | endfor | + | old_err_mean=err_mean; |
- | bias_gain+=pinv(c_matrix)*errors; | + | iterations++; |
- | err_mean=mean(abs(errors)); | + | printf(" |
- | if(iterations==1) | + | fflush(stdout); |
- | convergence=(err_mean/ | + | endwhile |
- | elseif(iterations> | + | |
- | convergence=0.66*convergence+0.34*(err_mean/ | + | |
- | endif | + | |
- | old_err_mean=err_mean; | + | |
- | iterations++; | + | |
- | printf(" | + | |
- | fflush(stdout); | + | |
- | endwhile | + | |
endfunction | endfunction | ||
</ | </ | ||
code/fit.1264634402.txt.gz · Last modified: 2010/01/27 23:20 by laurenceb