clc;
clear all;

T=zeros(3,3,63001);

for  t=0:2:126000
      
%艦船搖擺誤差角

pitchx=10*sin(pi*t/4+0.1)/57.3;  %俯仰角

rolly=5*sin(pi*t/5+0.01)/57.3;   %傾斜角即滾動角

yawz=6*sin(pi*t/6+0.02)/57.3;    %偏航角

%求微分方程

dpitchxdt=pi*2.5*cos(pi*t/4+0.1)/57.3; 

drollydt=pi*cos(pi*t/5+0.01)/57.3;   

dyawzdt=pi*cos(pi*t/6+0.02)/57.3;   

%捷聯矩陣/由b1坐標系轉到n坐標系

T(1,1,t/2+1)=cos(rolly)*cos(yawz)-sin(rolly)*sin(yawz)*sin(pitchx);
T(1,2,t/2+1)=-cos(pitchx)*sin(yawz);
T(1,3,t/2+1)=sin(rolly)*cos(yawz)+cos(rolly)*sin(pitchx)*sin(yawz);
T(2,1,t/2+1)=cos(rolly)*sin(yawz)+sin(rolly)*sin(pitchx)*cos(yawz);
T(2,2,t/2+1)=cos(pitchx)*cos(yawz);
T(2,3,t/2+1)=sin(rolly)*sin(yawz)-cos(rolly)*cos(yawz)*sin(pitchx);
T(3,1,t/2+1)=-sin(rolly)*cos(pitchx);
T(3,2,t/2+1)=sin(pitchx);
T(3,3,t/2+1)=cos(rolly)*cos(pitchx);

end

T1=zeros(3,63001);T2=zeros(3,63001);T3=zeros(3,63001);

T1=T(1,:,:);T2=T(2,:,:);T3=T(3,:,:);

fid1= fopen('E:\孫國偉\work\data41.txt','w');
    fprintf(fid1,'%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\r\n',T1(:,:));%,C(2,:),C(3,:),C(1,4,:),C(1,5,:),C(1,6,:),C(1,7,:),C(1,,:),C(1,9,:),C(1,10,:),C(1,11,:),C(1,12,:),C(1,13,:),C(1,14,:));
fclose(fid1);

fid2= fopen('E:\孫國偉\work\data42.txt','w');
    fprintf(fid2,'%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\r\n',T2(:,:));%,C(2,2,:),C(2,3,:),C(2,4,:),C(2,5,:),C(2,6,:),C(2,7,:),C(2,,:),C(2,9,:),C(2,10,:),C(2,11,:),C(2,12,:),C(2,13,:),C(2,14,:));
fclose(fid2);

fid3= fopen('E:\孫國偉\work\data43.txt','w');
    fprintf(fid3,'%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\t%20.14f\r\n',T3(:,:));%,C3(2,:),C3(3,:),C3(4,:),C3(5,:),C3(6,:),C3(7,:),C3(8,:),C3(9,:),C3(10,:),C3(11,:),C3(12,:),C3(13,:),C3(14,:));
fclose(fid3);