visible_sat_simu.m

用MATLAB编写的GPS导航系统可见卫星的仿真程序,可为GPS仿真人员提供参考

%模拟用户可见卫星

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;   %Unit is hour.


% Above is the User Position we give. and Current Time
% Because the Satellites' positions changing with Time.

global SatellitePosition
SatellitePosition=ones(24,4);
figure(1);
DrawEarth(0);
hold on;
DtoR=2*pi/360;
A1=[32.8   92.8   152.8   212.6    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;       %近地点的幅角。总共有6个卫星轨道平面，24个卫星轨道椭圆，但由于e很小，无法区分，因此画出6个椭圆表示
            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,:)];
            DrawSatellite(Ans(1,:),Ans(2,:),Ans(3,:),k);
            temp=k*m;
            SatellitePosition(temp,:)=[Ans(1,:) Ans(2,:) Ans(3,:) 1];
            hold on;
        end
    end
    
    %用余弦定理计算地心到用户和用户到卫星之间的夹角
    %如果两者之间的夹角小于90度，就认为该卫星是不可见的
    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';
        JiaJiao=acos((Dist1+Dist2-Dist3)/2/sqrt(Dist1)/sqrt(Dist2));
        if (JiaJiao<=pi/2)
            SatellitePosition(k,4)=0;
        end
    end
    figure(2);
    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,1);
   %标记出用户位置
   hold on;
   SatellitePosition=[SatellitePosition;UserPosition 0];
   %用变量CalUserPos来记录迭代计算结果
   CalUserPos=CalculateUserPosition(SatellitePosition);
   [m,n]=size(CalUserPos);
   for k=1:m
       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