fangzhenshuju.m

捷联惯导系统仿真数据生成工具

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);