clear;
i=1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
psi0=0;
lambda0=0;
h0=0;
A0=1;
A1=2;
A2=3;
w3=0.001*pi;
Wie=(15*pi/180)*3600;
Re=6378137;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
B0=1;
B1=1;
B2=1;
w0=0.01*pi;
w1=0.01*pi;
w2=0.01*pi;
T=0.025;%采样周期
time=100;%仿真时间
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=0:T:time
psi=psi0+A0*cos(w3*t);
lambda=lambda0+A1*cos(w3*t);
h=h0+A2*cos(w3*t);
psi1=-A0*w3*sin(w3*t);
lambda1=-A1*w3*sin(w3*t);
h1=-A2*w3*sin(w3*t);
psi11=-A0*w3*w3*cos(w3*t);
lambda11=-A1*w3*w3*cos(w3*t);
h11=-A2*w3*w3*cos(w3*t);
x=(Re+h)*cos(psi)*cos(lambda);
y=(Re+h)*cos(psi)*sin(lambda);
z=(Re+h)*sin(psi);
x1=h1*cos(psi)*cos(lambda)-psi1*(Re+h)*sin(psi)*cos(lambda)-lambda1*(Re+h)*cos(psi)*sin(lambda);
x11=(h11*cos(psi)*cos(lambda)-h1*psi1*sin(psi)*cos(lambda)-h1*lambda1*cos(psi)*sin(lambda))+(-psi11*(Re+h)*sin(psi)*cos(lambda)-h1*psi1*sin(psi)*cos(lambda)-(Re+h)*psi1*psi1*cos(psi)*cos(lambda)+(Re+h)*psi1*lambda1*sin(psi)*sin(lambda))+(-lambda11*(Re+h)*cos(psi)*sin(lambda)-h1*lambda1*cos(psi)*sin(lambda)+(Re+h)*psi1*lambda1*sin(psi)*sin(lambda)-(Re+h)*lambda1*lambda1*cos(psi)*cos(lambda));
y1=h1*cos(psi)*sin(lambda)-psi1*(Re+h)*sin(psi)*sin(lambda)+lambda1*(Re+h)*cos(psi)*cos(lambda);
y11=(h11*cos(psi)*sin(lambda)-h1*psi1*sin(psi)*sin(lambda)+h1*lambda1*cos(psi)*cos(lambda))+(-psi11*(Re+h)*sin(psi)*sin(lambda)-h1*psi1*sin(psi)*sin(lambda)-(Re+h)*psi1*psi1*cos(psi)*sin(lambda)-(Re+h)*psi1*lambda1*sin(psi)*cos(lambda))+(lambda11*(Re+h)*cos(psi)*cos(lambda)+h1*lambda1*cos(psi)*cos(lambda)-(Re+h)*psi1*lambda1*sin(psi)*cos(lambda)-(Re+h)*lambda1*lambda1*cos(psi)*sin(lambda));
z1=h1*sin(psi)+psi1*(Re+h)*cos(psi);
z11=(h11*sin(psi)+h1*psi1*cos(psi))+(psi11*(Re+h)*cos(psi)+h1*psi1*cos(psi)-psi11*psi11*(Re+h)*sin(psi));
Cet=[-sin(psi) cos(lambda) 0;
-sin(psi)*cos(lambda) -sin(psi)*sin(lambda) cos(psi);
cos(psi)*cos(lambda) cos(psi)*sin(lambda) sin(psi)];
Vett=Cet*[x1;y1;z1];
Vetxt=Vett(1,1);
Vetyt=Vett(2,1);
Vetzt=Vett(3,1);
Wett=[-Vetyt/(Re+h);Vetxt/(Re+h);Vetxt*tan(psi)/(Re+h)];
art=Cet*[x11;y11;z11];
at=art-cross(Wett,Vett);
Wiet=[0;Wie*cos(psi);Wie*sin(psi)];
gt=[0;0;9.8];
ft=at+cross((2*Wiet+Wett),Vett)-gt;
f(i,1)=ft(1);
f(i ,2)=ft(2);
f(i, 3)=ft(3);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
theta=B0*cos(w0*t);
gamma=B1*cos(w1*t);
phi=B2*cos(w2*t);
theta1=-B0*w0*sin(w0*t);
gamma1=-B1*w1*sin(w1*t);
phi1=-B2*w2*sin(w2*t);
Wtbb=[cos(gamma) 0 sin(gamma)*cos(theta);
0 1 -sin(theta) ;
sin(gamma) 0 -cos(gamma)*cos(theta);]*[theta1;gamma1;phi1];
Witt=Wiet+Wett;
Ctb=[cos(gamma)*cos(phi)+sin(gamma)*sin(theta)*sin(phi) sin(gamma)*sin(theta)*cos(phi)-cos(gamma)*sin(phi) -sin(gamma)*cos(theta);
sin(phi)*cos(theta) cos(theta)*cos(phi) sin(theta) ;
sin(gamma)*cos(phi)-cos(gamma)*sin(theta)*sin(phi) -sin(gamma)*sin(phi)-cos(gamma)*sin(theta)*cos(phi) cos(gamma)*cos(theta);];
Witb=Ctb*Witt;
Wibb=Witb+Wtbb;
W(i,1)=Wibb(1);
W(i ,2)=Wibb(2);
W(i, 3)=Wibb(3);
fb=Ctb*ft;
f(i,1)=fb(1);
f(i ,2)=fb(2);
f(i, 3)=fb(3);
psii(i,1)=psi;
lambdaa(i,1)=lambda;
hh(i,1)=h;
thetaa(i,1)=theta;
gammaa(i,1)=gamma;
phii(i,1)=phi;
i=i+1;
end
i=0;
for j=3:3:time/T
i=i+1;
WW(i,1)=W(j,1);
WW(i,2)=W(j,2);
WW(i,3)=W(j,3);
end
i=0;
for j=6:6:time/T
i=i+1;
psiii(i,1)=psii(j,1);
lambdaaa(i,1)=lambdaa(j,1);
thetaaa(i,1)=thetaa(j,1);
gammaaa(i,1)=gammaa(j,1);
phiii(i,1)=phii(j,1);
end
%plot(phii)
%plot(WW);
%figure;
%plot(f);
%figure;
%plot(lambdaaa);
%figure;
%plot(phiii);
fid=fopen('d:\WWx.dat','w');
fprintf(fid,'%.8f\n',WW(:,1));
fclose(fid);
fid=fopen('d:\WWy.dat','w');
fprintf(fid,'%.8f\n',WW(:,2));
fclose(fid);
fid=fopen('d:\WWz.dat','w');
fprintf(fid,'%.8f\n',WW(:,3));
fclose(fid);
fid=fopen('d:\fbx.dat','w');
fprintf(fid,'%.8f\n',f(:,1));
fclose(fid);
fid=fopen('d:\fby.dat','w');
fprintf(fid,'%.8f\n',f(:,2));
fclose(fid);
fid=fopen('d:\fbz.dat','w');
fprintf(fid,'%.8f\n',f(:,3));
fclose(fid);
