gps_constellation_emluator.m

本程序设计了GPS卫星仿真器

%%%%% gps constellation emluator %%%%%%
clear
% constant defination%                 
pi=3.1415926;
a=26609e3;            %轨道长半轴长，单位已经换算为 m
e=0.006;              %轨道的偏心率
i_0=55*pi/180;          %基准时间t_0的轨道倾角
% gpa 采用的椭球参数 %
a_e=6378137;          %地球椭球的长半径
f_e=1/298.257223563;  %地球椭球体扁率
e_2=2*f_e-f_e^2;      %GPS参考椭球第一偏心率的平方

%测站在大地坐标系中的经纬度数据,经度L,纬度B,高度H %
station=[118;32;300];
station(1,1)=station(1,1)*pi/180;%完成弧度转换
station(2,1)=station(2,1)*pi/180;

L=station(1,1);
B=station(2,1);
H=station(3,1);
%计算椭球的卯酉圈曲率半径N
W=sqrt(1-e_2*sin(B)^2);
N=a_e/W;
%将测站在大地坐标系下的坐标转换为地球坐标系的空间直角坐标[xp,yp,zp]
xp=(N+H)*cos(B)*cos(L);
yp=(N+H)*cos(B)*sin(L);
zp=(N*(1-e_2)+H)*sin(B);
%求系数阵h
h(1,1)=-sin(B)*cos(L);
h(1,2)=-sin(B)*sin(L);
h(1,3)=cos(B);

h(2,1)=-sin(L);
h(2,2)=cos(L);
h(2,3)=0;

h(3,1)=cos(B)*cos(L);
h(3,2)=cos(B)*sin(L);
h(3,3)=sin(B);

E0=15;
mu=3.986008e14;       %开普勒常数，单位为m3/s2
w_ie=7.292115147e-5;  %地球自转平均角速率，单位rad/s
% 卫星轨道参数矩阵epoch:2007-04-01 14:21:46，第一列卫星标号1~30，第二列升交点赤经W_0，第三列平近点角距M_0 %
sate=[1 12.4664 313.6181;2 13.2561 99.1277;3 10.4480 41.9519;4 12.4727 289.5653;5 11.6077 116.5486;6 10.5432 209.4507;
      7 65.1275 73.1881;8 66.9823 101.9770;9 68.3454 286.8979;10 73.1479 200.4810;11 70.2154 77.1527;
      12 125.5712 295.1060;13 128.3618 285.5386;14 131.8240 123.7737;15 131.3074 44.5724;16 130.3997 73.6977;
      17 186.9655 97.3078;18 188.4401 98.7626;19 184.9619 319.1793;20 194.1995 55.7345;21 190.8460 172.0155;
      22 253.5063 46.1491;23 251.3094 346.3001;24 241.0610 333.6671;25 252.3879 163.3319;26 250.2394 231.0366;
      27 311.8862 334.8484;28 309.8682 287.5291;29 312.8828 146.8152;30 313.3067 94.0084];
%w=;                    %近地点角
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t_0=0;                %星历的参考历元
t_k=0;                %仿真时间,单位s
a3=a^3;
n=sqrt(mu/a3);       % n=(2*pi)/T=sqrt(mu/a3),应用了开普勒第三定律
i=1;
r=1;
for j=1:30
     sate(j,3)=sate(j,3)*pi/180;%平近点角
     sate(j,2)=sate(j,2)*pi/180;%升交点赤经
end
s=43201;
dot_step=300;
q=0;
while t_k<s     %共计11小时58分
    q=q+1;        %各个矩阵的行数表示量
    t_k
    t(q,1)=t_k;
    j=1;            % 卫星标号
    while j<=30     %各个矩阵的列数表示量
          M_k(q,j)=sate(j,3)+n*(t_k-t_0);    
       %%%%%%%%%%%%% 求偏近点角 E_k %%%%%%%%%%%%% 
          Et_1(q,j)=M_k(q,j);
          t_end=1;  
          while(t_end)
              Et(q,j)=M_k(q,j)+e*sin(Et_1(q,j));        
              delta_E(q,j)=Et(q,j)-Et_1(q,j);
              Et_1(q,j)=Et(q,j);
              if abs(delta_E(q,j))<=1.0e-6
                 E_k(q,j)=Et(q,j);  
                 t_end=0;
              end 
          end 
       %%%%%%%%%%%%%% 求真近点角 f 的值 %%%%%%%%%%
          %A=(cos(E_k(q,j))-e)/(1-e*cos(E_k(q,j)));          %% f 的余弦
          A=cos(E_k(q,j))-e;               %分母一定是是大于0的数,所以只取分子来做判断
          %B=(sin(E_k(q,j))*sqrt(1-e^2))/(1-e*cos(E_k(q,j)));%% f 的正弦
          B=sqrt(1-e^2)*sin(E_k(q,j));

         if (A==0)
             f(q,j)=pi/2;            
         elseif (B==0)
             f(q,j)=pi;
         else
             f(q,j)=atan(abs(B/A));
             if ((B>0)&(A<0))
                 f(q,j)=pi-f(q,j);
             elseif ((B<0)&(A<0))
                 f(q,j)=pi+f(q,j);
             elseif ((B<0)&(A>0))
                 f(q,j)=2*pi-f(q,j);
             end
         end
                           
         % 计算升交点角距u_k,地心距r_k,和轨道倾角i_k ,升交点的经度L_k %
         % u_k(q,j)=w+f(q,j)+c_uc*cos(2*(w+f(q,j)))+c_us*sin(2*(w+f(q,j)));
         u_k(q,j)=f(q,j);         
         % r_k(q,j)=a*(1-e*cos(E_k(q,j)))+c_rc*cos(2*(w+f(q,j)))+c_rs*sin(2*(w+f(q,j)));
         r_k(q,j)=a*(1-e*cos(E_k(q,j)));    
         % i_k(q,j)=i_0+di_k+c_ic*cos(2*(w+f(q,j)))+c_is*sin(2*(w+f(q,j)));
         i_k(q,j)=i_0;    
         % L_k(q,j)=sate(j,2)+(dW-w_ie)*(t_k-t_0)-w_ie*t_0;
         L_k(q,j)=sate(j,2);
         % L_k(q,j)=sate(j,2)-w_ie*(t_k);%%%%%%%%%%%  “＋”还是 “－” ？？
         %%%%%计算卫星在轨道直角坐标系中的坐标%%%%%%
         X_k(q,j)=r_k(q,j)*cos(u_k(q,j));
         Y_k(q,j)=r_k(q,j)*sin(u_k(q,j));
         Z_k(q,j)=0;
         % 计算导航星的位置,在地心坐标系中,ECEF(Earth-Centered Earth-Fixed)坐标系 %
         x_k(q,j)=r_k(q,j)*cos(u_k(q,j))*cos(L_k(q,j))-r_k(q,j)*sin(u_k(q,j))*sin(L_k(q,j))*cos(i_k(q,j));    
         y_k(q,j)=r_k(q,j)*cos(u_k(q,j))*sin(L_k(q,j))+r_k(q,j)*sin(u_k(q,j))*cos(L_k(q,j))*cos(i_k(q,j));
         z_k(q,j)=r_k(q,j)*sin(u_k(q,j))*sin(i_k(q,j));
      %%%%%%计算仰角 E=arctan(Z/sqrt(X^2+Y^2)) ,E_rad单位rad ,E_deg单位度 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
         delta_x(q,j)=x_k(q,j)-xp;
         delta_y(q,j)=y_k(q,j)-yp;
         delta_z(q,j)=z_k(q,j)-zp;
         %求卫星在 %%%  站心坐标系下 %%%% 的坐标
         X_sta(q,j)=h(1,1)*delta_x(q,j)+h(1,2)*delta_y(q,j)+h(1,3)*delta_z(q,j);
         Y_sta(q,j)=h(2,1)*delta_x(q,j)+h(2,2)*delta_y(q,j)+h(2,3)*delta_z(q,j);
         Z_sta(q,j)=h(3,1)*delta_x(q,j)+h(3,2)*delta_y(q,j)+h(3,3)*delta_z(q,j);
         %%%%%%%%%%%%%%%%%%%
         E_deno(q,j)=X_sta(q,j)^2+Y_sta(q,j)^2;
         E_deno(q,j)=sqrt(E_deno(q,j));
         if E_deno(q,j)==0
            E_rad(q,j)=pi/2;
            E_deg(q,j)=90;
         else
            E_rad(q,j)=atan(Z_sta(q,j)/E_deno(q,j));
            E_deg(q,j)=E_rad(q,j)*180/pi;
         end
%%%% 开始高度角比较 ,E0 为给定的高度角初始值 %%%%         
         ele(q,j)=E_deg(q,j);
         if ele(q,j)>=E0
             ele(q,j)=1;
             if r~=q
                 i=1;
                 r=q;
             end
  % s_n 为 q 时刻(比真实时刻 t_k 加 1 )可见星的标号的矩阵
             s_n(q,i)=j;
             i=i+1;
         else
             ele(q,j)=0;
         end
         j=j+1;
    end
    t_k=t_k+dot_step;
end
q_end=q
%%%%%%%% 计算可见星个数 sum_s %%%%%
for q=1:q_end
    sum_s(q,1)=0;
    for j=1:30
        sum_s(q,1)=sum_s(q,1)+ele(q,j);
        j=j+1;
    end
end
%%-------------------------------以下计算 DOP 值-----------------------------------%%
for q=1:q_end
        kk=sum_s(q,1);%gps可见星个数
        for i=1:kk
            sd(i)=s_n(q,i);% sd矩阵为可见星标号阵
            %% 以下在站心坐标系下计算 几何距离R %   
            R(q,i)=sqrt((x_k(q,sd(i))-xp)^2 + (y_k(q,sd(i))-yp)^2 + (z_k(q,sd(i))-zp)^2);
            %%%%%  求解用户位置变化量 %%%%%%
            %  求A,V=AX+L %
            a_11(i,q)=(x_k(q,sd(i))-xp)/R(q,i);a_12(i,q)=(y_k(q,sd(i))-yp)/R(q,i);
            a_13(i,q)=(z_k(q,sd(i))-zp)/R(q,i);a_14(i,q)=1;
            A_rou(i,:)=[-a_11(i,q),-a_12(i,q),-a_13(i,q),a_14(i,q)];
            R(q,i)=0;
        end 
        %
        delta_user(:,:)=inv(A_rou'*A_rou);
        % ---计算PDOP值--%
        PDOP(1,q)=sqrt(delta_user(1,1)+delta_user(2,2)+delta_user(3,3));
end
fh_numstar=figure('Name','visible satellite','NumberTitle','off');
    plot(t,sum_s(:,1),'m.-');
    title('可见星数目仿真');
    xlabel('t(sec)');ylabel('可见星数目n');
    ylim([0,30]);
    grid;
fh_pdop=figure('Name','PDOP Value','NumberTitle','off');
    plot(t,PDOP(1,:),'m.');
    title('PDOP值仿真');
    xlabel('t(sec)');ylabel('PDOP');
    ylim([0,15]);
    grid;
fh_orbit=figure('Name','Orbit','NumberTitle','off');
    plot3(x_k(:,1),y_k(:,1),z_k(:,1),'m',X_k(:,1),Y_k(:,1),Z_k(:,1),'b');
    xlabel('x坐标/m');ylabel('y坐标/m');zlabel('z坐标/m');
    title('粉红曲线为卫星01在天球坐标系位置,蓝色曲线卫星01在轨道直角坐标系位置');
    grid;