code:7_state_ekf_with_bias
This is an old revision of the document!
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/7_state_ekf_with_bias.1263082325.txt.gz · Last modified: 2010/01/10 00:12 by laurenceb