UKHAS Wiki

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.

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.

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