clear all
clc;
clear;
% close all;
path='C:\Users\Administrator\Desktop\liu\9.9.1';
file='.sen';
filename=strcat(path,file);
fp=fopen(filename,'rb');
b=fread(fp,inf,'double');
lsize=size(b);
for i=0:(lsize(1,1)/11-1)
DATA((i+1),1:4)=b((11*i+8):(11*i+11)); %b(11*i+2:4);
DATA((i+1),5:7)=b((11*i+5):(11*i+7));
end
% Ctb = [e0^2+e1^2-e2^2-e3^2 2*(e1*e2+e0*e3) 2*(e1*e3-e0*e2);% > 用四元素表示得姿态矩阵
% 2*(e1*e2-e0*e3) e0^2-e1^2+e2^2-e3^2 2*(e2*e3+e0*e1);
% 2*(e1*e3+e0*e2) 2*(e2*e3-e0*e1) e0^2-e1^2-e2^2+e3^2];
% %> 姿态角计算
% Seita(i) = asin(Ctb(2,3)); %> 俯仰角计算
% Gama(i) = atan(-Ctb(1,3)/Ctb(3,3)); %> 横滚角计算
% if abs(Ctb(3,3))>eps
% Gama(i) = atan(-Ctb(1,3)/Ctb(3,3));
% if Ctb(3,3)>0
% Gama(i) = Gama(i);
% elseif -Ctb(1,3)> 0
% Gama(i) = Gama(i)+pi;
% else Gama(i) = Gama(i)-pi;
% end
% elseif -Ctb(1,3)> 0
% Gama(i) = pi/2;
% else Gama(i) = -pi/2;
% end
% Ksai(i) = atan(Ctb(2,1)/Ctb(2,2));% > 航向角计算
% if abs(Ctb(2,2))>eps
% Ksai(i) = atan(Ctb(2,1)/Ctb(2,2));
% if Ctb(2,2)>0
% Ksai(i) = Ksai(i);
% elseif Ctb(2,1)> 0
% Ksai(i) = Ksai(i)+pi;
% else Ksai(i) = Ksai(i)-pi;
% end
% elseif Ctb(2,1)>0
% Ksai(i) = pi/2;
% else Ksai(i) = -pi/2;
% end
% end %> 将弧度换算为角度
% Seita = Seita*180/pi;Gama = Gama*180/pi;Ksai = Ksai*180/pi;
% L = L*180/pi;Lamda = Lamda*180/pi;
for i=1:size(DATA,1)
q2 = DATA(i,1);
q3 = DATA(i,2);
q4 = DATA(i,3);
q1 = DATA(i,4);
% q1 = 0.1;
% q2 = 0.1;
% q3 = 0.1;
% q4 = 0.1;
T=[q1^2+q2^2-q3^2-q4^2,2*(q2*q3-q1*q4),2*(q2*q4+q1*q3);
2*(q2*q3+q1*q4),q1^2-q2^2+q3^2-q4^2,2*(q3*q4-q1*q2);
2*(q2*q4-q1*q3),2*(q3*q4+q1*q2),q1^2-q2^2-q3^2+q4^2];
T=T';
fuyang=asin(T(2,3)); %俯仰角值为其主值
gunzhuanm=atan(-T(1,3)/T(3,3));
hangxiangm=atan(T(1,2)/T(2,2)); %三个姿态角的主值
if T(3,3)>0;
gunzhuan=gunzhuanm;
elseif (T(3,3)<0)&(gunzhuanm<0);
gunzhuan=gunzhuanm+pi;
else (T(3,3)<0)&(gunzhuanm>0);
gunzhuan=gunzhuanm-pi;
end %滚转角
if (T(2,2)>0)&(T(1,2)>0)
hangxiang=hangxiangm;
elseif (T(2,2)>0)&(T(1,2)<0)
hangxiang=hangxiangm;
elseif (T(2,2)<0)&(T(1,2)>0)
hangxiang=hangxiangm+pi;
else (T(2,2)<0)&(T(1,2)<0)
hangxiang=hangxiangm-pi;
end
% if (T(2,2)>0)&(hangxiangm>0);
% hangxiang=hangxiangm;
% elseif (T(2,2)>0)&(hangxiangm<0);
% hangxiang=hangxiangm+2*pi;
% else T(2,2)<0;
% hangxiang=hangxiangm+pi;
% end %航向角
% if (T(2,2) < 0 && T(1,2) > 0)
% my_yaw(i) = my_yaw(i) + 180;
% end
% if (T(2,2) < 0 && T(1,2) < 0)
% my_yaw(i) = my_yaw(i) - 180;
% end
hangxiangM(i)=hangxiang*180/pi;
fuyangM(i)=fuyang*180/pi;
gunzhuanM(i)=gunzhuan*180/pi;
end
figure(1)
subplot(3,1,1)
plot(hangxiangM)
title('航向角')
subplot(3,1,2)
plot(fuyangM)
title('俯仰角')
subplot(3,1,3)
plot(gunzhuanM)
title('滚动')
figure(2)
subplot(4,1,1)
plot(DATA(:,1))
title('q2')
subplot(4,1,2)
plot(DATA(:,2))
title('q3')
subplot(4,1,3)
plot(DATA(:,3))
title('q4')
subplot(4,1,4)
plot(DATA(:,4))
title('q1')
figure(2)
subplot(3,1,1)
plot(DATA(:,5)*180/pi)
subplot(3,1,2)
plot(DATA(:,6)*180/pi)
subplot(3,1,3)
plot(DATA(:,7)*180/pi)
评论0