Differences

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

--- code:7_state_ekf_with_bias [2010/01/10 00:12] – created laurenceb
+++ code:7_state_ekf_with_bias [2010/01/10 00:23] (current) – laurenceb
@@ Line 1: / Line 1: @@
-<code matlab>
-function quat_IMU_bias(beta,alpha,q,b)
+===== Bias tracking in Octave =====
+ This has been tested with three axes of accel and gyro, adding a magnetometer will give a dramatic improvement as absolute yaw determination will be possible. Interestingly this filter is still able to filter out yaw gyro bias errors if the IMU is tilted - i.e. any roll or pitch.
+In the first plot you can see an artiffically introduced large yaw bias being corrected reasonably well after the first few manovers. Angles are in radians.
+{{code:bigbias.png|}}
+ This second test uses raw test data with a sparkfun 6DOF IMU - i.e. actual biases. The intialisation of the filter was just guesstimated, and it takes a while to settle. However the values after 40 seconds are within 15% of the bias values obtained by interpolating between the gyro outputs when the IMU was stationary between movements.
+{{code:biases.png|}}
+===== GNU Octave code =====
+<code matlab>
+function quat_IMU_bias(beta,alpha,q,b)
 %constants---------------------------------------
 %beta=3e-6; - gyroscope noise
 %alpha=1e-4;	- accelerometer noise
 %q=4e-2 - quaternion process noise
 %b=1e-10 - gyro bias process noise
 % ^ these are sensible values
 #gyro_null=[0.0183;0.0101;0.0063];
 gyro_gain=[1/1.3096,0,0;0,1/1.41875,0;0,0,1/1.54607];
 delta_time=0.02857; %35 Hz sampling in this data set
 %read in the data - each data type is in a seperate row
 fp=fopen('rpy90bothways.TXT','r');
 data=fscanf(fp,'%f    %f    %f    %f    %f    %f    %f    %f    %f    %f',[10,inf]);
 fclose(fp);
 j=size(data);
 j=j(1,2);
 x_nav=[0;0;0];
 v_nav=[0;0;0];
 biases=[0;0;0];
 %setup Kalman filter
 x=[1;0;0;0;0;0;0];
 P=[pi^2,0,0,0,0,0,0;0,pi^2,0,0,0,0,0;0,0,pi^2,0,0,0,0;0,0,0,pi^2,0,0,0;0,0,0,0,5e-7,0,0;0,0,0,0,0,5e-7,0;0,0,0,0,0,0,5e-7];
 q/=35;
 Q=[q,0,0,0,0,0,0;0,q,0,0,0,0,0;0,0,q,0,0,0,0;0,0,0,q,0,0,0;0,0,0,0,b,0,0;0,0,0,0,0,b,0;0,0,0,0,0,0,b];
 %main loop---------------------------------------
 for n=1:j;
     %takes vectors g and a as inputs for gyros and accels, g needs to be
     %premultiplied by delta time per run.
     g=gyro_gain*data(2:4,n)*delta_time-x(5:7)-[0;0;0.001];
     %get the accel vecor
     a=data(5:7,n);
     %a=-a for some of the sensors on the dataset;
     a(2)=-a(2);
     a(1)=-a(1);
     %normalise acceleration vector
     a=a/sqrt(a(1)^2+a(2)^2+a(3)^2);
     %accel sensor measurement jacobian
     H=[2*x(3),2*x(4),2*x(1),2*x(2),0,0,0;-2*x(2),-2*x(1),2*x(4),2*x(3),0,0,0;2*x(1),-2*x(2),-2*x(3),2*x(4),0,0,0];
     %misalignment error jacobian
     F=[1,-g(1)/2,-g(2)/2,-g(3)/2,x(2)/2,x(3)/2,x(4)/2;g(1)/2,1,g(3)/2,-g(2)/2,-x(1)/2,x(4)/2,-x(3)/2;g(2)/2,-g(3)/2,1,g(1)/2,-x(4)/2,-x(1)/2,x(2)/2;g(3)/2,g(2)/2,-g(1)/2,1,x(3)/2,-x(2)/2,-x(1)/2;0,0,0,0,1,0,0;0,0,0,0,0,1,0;0,0,0,0,0,0,1];
     %input error jacobian
     E=0.5*[-x(2),-x(3),-x(4);x(1),-x(4),x(3);x(4),x(1),-x(2);x(3),x(2),x(1);0,0,0;0,0,0;0,0,0];
     %propogate the state vector note the inclusion of 2*I
     x=0.5*[2,-g(1),-g(2),-g(3),0,0,0;g(1),2,g(3),-g(2),0,0,0;g(2),-g(3),2,g(1),0,0,0;g(3),g(2),-g(1),2,0,0,0;0,0,0,0,2,0,0;0,0,0,0,0,2,0;0,0,0,0,0,0,2]*x;
     %normalise the quaternion
     x(1:4)/=sqrt(x(1)^2+x(2)^2+x(3)^2+x(4)^2);
     %propogate the covarience, can also use P+=, and subtract identity from F
     P=F*P*F'+E*E'*beta+Q;
     %find the residual
     y=a-[2*(x(2)*x(4)+x(1)*x(3));2*(x(3)*x(4)-x(1)*x(2));x(1)^2-x(2)^2-x(3)^2+x(4)^2];
     fflush(stdout);
     %alpha is accel noise
     S=H*P*H'+alpha*[1,0,0;0,1,0;0,0,1];
     %find gain
     K=P*H'*S^(-1);
     %state update
     x=x+(K*y);
     %covariance update
     P=P-K*H*P;
     %normalise quaternion
     x(1:4)/=sqrt(x(1)^2+x(2)^2+x(3)^2+x(4)^2);
     %work out the roll pitch and yaw (these are NOT EULER ANGLES)
@@ Line 129: / Line 89: @@
 	p_x=-p_x;
     endif
     yaw(n)=asin(norm(cross(trans_x,[p_x;0;p_z])));%asin the lenght to get the rotation angle
     if trans_x(2)<0			%account for ambiguity in sign caused by taking norm - check sign of y co-ord of transformed x
 	yaw(n)=-yaw(n);
     endif
     if trans_x(1)<0    			%mapping of x onto x is negative
         yaw(n)=pi-yaw(n);               %account for the ambiguity in asin
 	if(yaw(n)>pi)			%stay in range +-pi
 		yaw(n)-=2*pi;
 	endif
     endif
     if z_z<0	%this if for the case where we are inverted - the body z axis is in -ive z direction so reverse yaw
         yaw(n)=-yaw(n);
     endif
     biases=[biases,x(5:7)/delta_time];
 endfor
 plot(data(1,1:j),roll(1:j),"r",data(1,1:j),pitch(1:j),"g",data(1,1:j),yaw(1:j),"b");
 h = legend('Roll','Pitch','Yaw',3);
 set(h,'Interpreter','none');
 #plot(data(1,1:j),biases(1,1:j),"r",data(1,1:j),biases(2,1:j),"g",data(1,1:j),biases(3,1:j),"b");
 endfunction
 </code>