===== 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)
    roll(n)=asin(2*x(1)*x(2)+2*x(3)*x(4));
    trans_x=[x(1)^2+x(2)^2-x(3)^2-x(4)^2;2*x(1)*x(4)+2*x(2)*x(3);2*x(2)*x(4)-2*x(1)*x(3)];
    pitch(n)=asin(trans_x(3));
    z_x=2*(x(2)*x(4)+x(1)*x(3));	%projection of z axis onto x axis
    z_z=x(1)^2-x(2)^2-x(3)^2+x(4)^2;	%projection of z axis onto z axis
    p_z=z_x/norm([z_z;z_x]);		%the body x axis rotated so as to have 0 y component
    p_x=sqrt(1-p_z^2);
    if(z_z>0)				%sign ambiguity
	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>