Differences

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

--- 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.
 <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('speedevilaccel.txt','r');
+	datalenght=size(data)(2)
-data=fscanf(fp,'%f    %f    %f    %f    %f',[5,inf]);
+	%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)
-size(data);
+	bias_gain=initial;
-accel=data(3:5,:);
+	convergence=0.5;iterations=0;
-datalenght=size(data)(2);
+	while(convergence<0.99 || convergence>1)
-bias_gain=[0.001;0.001;0.001;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));
-convergence=0.5;iterations=0;
+		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(convergence<0.95 || convergence>1)
+*bias_gain(3)*(data(3,:)-bias_gain(6)).^2;-2*(bias_gain(1)^2)*(data(1,:)-bias_gain(4));...
-	err_mean=0;
+		-2*(bias_gain(2)^2)*(data(2,:)-bias_gain(5));-2*(bias_gain(3)^2)*(data(3,:)-bias_gain(6))];
-	correction_total=zeros(6,1);
+		bias_gain+=pinv(corrections')*errors'./2;	%note the factor of 1/2 - this increases stability
-	for(i=1:datalenght)
+		err_mean=sqrt(mean(abs(errors))+1)-1;
-		measured=[bias_gain(1),0,0;0,bias_gain(2),0;0,0,bias_gain(3)]*(accel(1:3,i)-bias_gain(4:6));
+		if(iterations==1)
-		erro=1-dot(measured,measured);
+			convergence=(err_mean/old_err_mean);	%we iterate until we stop converging
-		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))];
+		elseif(iterations>1)
-		if(i==1)
+			convergence=0.66*convergence+0.34*(err_mean/old_err_mean);
-			c_matrix=correction;
-			errors=erro;
-		else
-			c_matrix=[c_matrix;correction];
-			errors=[errors;erro];
 		endif
-	endfor
+		old_err_mean=err_mean;
-	bias_gain+=pinv(c_matrix)*errors;
+		iterations++;
-	err_mean=mean(abs(errors));
+		printf("Convergence: %f, Residual: %f\n",convergence,err_mean);
-	if(iterations==1)
+		fflush(stdout);
-		convergence=(err_mean/old_err_mean);
+	endwhile
-	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>