Differences

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

--- 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 ======
 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>
-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)
+*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
 </code>