function quat_IMU_bias(beta,alpha,q,b)
%constants---------------------------------------
%beta=0.000003; - gyroscope noise
%alpha=0.0001; - accelerometer noise
%q=0.04 - quaternion process noise
%b=0.0000000001 - 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];
%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,0.0001,0,0;0,0,0,0,0,0.0001,0;0,0,0,0,0,0,0.0001];
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);
%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
x(5:7)
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');
endfunction