【运动学】基于matlab GPS卫星仿真.zip

clear;clc;close all; a=26560; e=0.02; E=[0:0.1:2*pi]; x=a*(cos(E)-e); y=a*sqrt((1-e^2))*sin(E); z=0*E; TimeNow=0; global SatellitePosition SatellitePosition=ones(24,4); figure(1); DrawEarth(0); hold on; DtoR=2*pi/360; A1=[32.8 92.8 152.8 212.8 272.8 332.8 ]; DrawSatelliteOrbit; Ctable=[10 50 160 260 ; 80 180 220 320 ; 10 130 250 340; 50 150 170 300; 100 210 310 340 ; 140 150 240 350;]; Wx=ones(1,1);Wy=ones(1,1);Wz=ones(1,1); simple=1; for k=1:6 A=A1(k)*DtoR; B=55*DtoR; for m=1:4 C=Ctable(k,m)*DtoR+TimeNow*2*pi/24; x=a*(cos(C)-e); y=a*sqrt((1-e^2))*sin( C); z=0*C; R3=[cos(A) -sin(A) 0; sin(A) cos(A) 0; 0 0 1;]; R1=[1 0 0; 0 cos(B) -sin(B); 0 sin(B) cos(B);]; R2=[cos(C) -sin(C) 0; sin(C) cos(C) 0; 0 0 1;]; L1=length(E); R312=R3*R1*R2; Ans=R312*[x;y;z;]; Wx=[Wx Ans(1,:)]; Wy=[Wy Ans(2,:)]; Wz=[Wz Ans(3,:)]; x1=Ans(1,:); y1=Ans(2,:); z1=Ans(3,:); DrawSatellite(x1,y1,z1,k); temp=k*m; SatellitePosition(temp,:)=[x1 y1 z1 1] hold on; end end EarthCenterPos=[0 0 0]; UserPosition=[6400 0 0]; for k=1:24 Temp=SatellitePosition(k,1:3)-UserPosition; Dist1=Temp*Temp'; Temp=UserPosition-EarthCenterPos; Dist2=Temp*Temp'; Temp=SatellitePosition(k,1:3)-EarthCenterPos; Dist3=Temp*Temp'; JiaoJiao=acos((Dist1+Dist2-Dist3)/2/sqrt(Dist1)/sqrt(Dist2)); if(JiaoJiao<=pi/2) SatellitePosition(k,4)=0; end end figure(2) DrawEarth(0); hold on; DrawEarth(0); hold on; DrawSatelliteOrbit; for k=1:24 if(SatellitePosition(k,4)==1) TempX=SatellitePosition(k,1); TempY=SatellitePosition(k,2); TempZ=SatellitePosition(k,3); DrawSatellite(TempX,TempY,TempZ,1); end end TempX=UserPosition(1); TempY=UserPosition(2); TempZ=UserPosition(3); Cube=50; BoxPlot3(TempX,TempY,TempZ,Cube,Cube,3*Cube,3); hold on; SatellitePosition=[SatellitePosition;UserPosition 0]; %CalUserPos=CalculateUserPosition(SatellitePosition); %[m,n]=size(CalUserPos); %% TempX=CalUserPos(k,1); % TempY=CalUserPos(k,2); %TempZ=CalUserPos(k,3); %Cube=50; %BoxPlot3(TempX,TempY,TempZ,Cube,Cube,3*Cube,3); %hold on; %end