A few matlab scripts for attitude and hopefully (at some point) position estimation using MEMS sensors. The data used for tests was kindly recorded by a member of the sparkfun forum with a sparkfun 6DOF IMU. Ascii data is here. The format is time (seconds) roll pitch and yaw gyro rates, and x,y,z accel readings in g (roughly - obviously gain and bias need to be applied).

It is possible to add three gyro biases to the state vector, this can give improved results even without a magnetometer to correct yaw.

This is the pure data from the IMU, read the code to find derived bias and gain values for the sensors.

The quaternion state vector was converted to elevations relative to the artificial horizon - the data you would actually need to input into PID loop(s) controlling the airframe. The “glitch” at approx 51 seconds is due to the fact the IMU is effectively in a “dive” so the Yaw becomes relatively ambiguous - a bit of noise is enough to force the projection of the end of the bodies x axis on the xy plane (found as part of the calculation process) into the opposite xy plane quadrant and cause a heading shift of pi. This is a real effect - the ground track of an aircraft in this situation really would reverse direction i.e. change by pi radians - think of a plane in a vertical loop.

The horizon elevations between motions are less than +-2 degrees, which is well within the limits of the experiment (IMU was handheld). Also interesting is the low drift of the “Yaw” (more correctly a heading in this graph), despite the fact there is no magnetometer to provide a second fix vector.

This simply uses guesstimated accelerometer biases and gains, and was calculated independently from the filter, so is very sub optimal. However the IMU can be seen to be lifted off the table and placed back down. The large sideways drifts are due to inaccurate gain and bias values preventing g from being cancelled out correctly, and also slightly decreasing the absolute accuracy of the attitude solution during sharp turns. Nevertheless, the position is quite stable over a period of 1.5 minutes, quite impressive for MEMS.

The second plot shows the results with velocity decaying at approx exp(-0.3t) where t is time in seconds.

This requires an ascii input file, see the description at the top of the page.

function quat_IMU_simple(beta,alpha,q)
 
%constants---------------------------------------
%beta=0.000003;
%alpha=0.0001;
%q=0.04 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];
P=[pi^2,0,0,0;0,pi^2,0,0;0,0,pi^2,0;0,0,0,pi^2];
Q=(q/35)*[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1];
%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)-gyro_null)*delta_time;
    %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);-2*x(2),-2*x(1),2*x(4),2*x(3);2*x(1),-2*x(2),-2*x(3),2*x(4)];
    %misalignment error jacobian
    F=[1,-g(1)/2,-g(2)/2,-g(3)/2;g(1)/2,1,g(3)/2,-g(2)/2;g(2)/2,-g(3)/2,1,g(1)/2;g(3)/2,g(2)/2,-g(1)/2,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)];
    %propogate the state vector
    x=0.5*[2,-g(1),-g(2),-g(3);g(1),2,g(3),-g(2);g(2),-g(3),2,g(1);g(3),g(2),-g(1),2]*x;
    %normalise the quaternion
    x=x/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=x/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
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