bd_gg_fixpoint_num_s.m

GPS,GALILEO,北斗可见星数目仿真

%%%%%%%%%%%%%%%%%% gps constellation emluator %%%%%%%%%%%

clear
% constant defination%                 
pi=3.1415926;
a_1=26609e3;            %gps轨道长半轴长，单位已经换算为 m
a_2=29994e3;            %galileo轨道长半轴长
e_1=0.006;              %轨道的偏心率
e_2=0;              
i_0_gps=55*pi/180;        %gps基准时间t_0的轨道倾角
i_0_galileo=56*pi/180;    %galileo基准时间t_0的轨道倾角
% gps 采用的椭球参数%
a_e=6378137;          %地球椭球的长半径
e_e=1/298.257223563;  %地球椭球的第一偏心率
% galileo 采用的椭球参数 %
a_e_2=6378136.49;    
e_e_2=1/298.25645;  
%测站在大地坐标系中的经纬度数据,经度L,纬度B,高度H %
station=[118;32;15];
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_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_e^2)+H)*sin(B);
%计算galileo椭球的卯酉圈曲率半径N
W=sqrt(1-e_e_2^2*sin(B)^2);
N=a_e_2/W;
xp_2=(N+H)*cos(B)*cos(L);
yp_2=(N+H)*cos(B)*sin(L);
zp_2=(N*(1-e_e_2^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);
%给出三颗定位卫星的经、纬、高
BD(1,:)=[80,0,36000000];    
BD(2,:)=[140,0,36000000];
BD(3,:)=[110.5,0,36000000];
% 大地坐标系---------->空间直角坐标
for i=1:3
    L=BD(i,1)*pi/180;%弧度转换
    B=BD(i,2)*pi/180;
    H=BD(i,3);
    % 计算椭球的卯酉圈曲率半径N
    W=sqrt(1-e_e^2*sin(B)^2);
    N=a_e/W;
    X_BD(i,1)=(N+H)*cos(B)*cos(L);
    Y_BD(i,1)=(N+H)*cos(B)*sin(L);
    Z_BD(i,1)=(N*(1-e_e^2)+H)*sin(B);
end
E0=15;
mu=3.986008e14;       %开普勒常数，单位为m3/s2
w_ie=7.292115147e-5;  %地球自转平均角速率，单位rad/s
% 卫星轨道参数矩阵，第一列卫星标号1~24，第二列升交点赤经W_0，第三列平近点角距M_0 %
sate_1=[1 325.73 190.96;2 325.73 220.48;3 325.73 330.17;4 325.73 83.58;                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  
      5 25.73 249.90;6 25.73 352.12;7 25.73 25.25;8 25.73 124.10;
      9 85.73 286.20;10 85.73 48.94;11 85.73 155.08;12 85.73 183.71;
      13 145.73 312.30;14 145.73 340.93;15 145.73 87.06;16 145.73 209.81;
      17 205.73 11.90;18 205.73 110.76;19 205.73 143.88;20 205.73 246.11;
      21 265.73 52.42;22 265.73 165.83;23 265.73 275.52;24 265.73 305.04];
for j=1:24
     sate_1(j,3)=sate_1(j,3)*pi/180;%平近点角
     sate_1(j,2)=sate_1(j,2)*pi/180;%升交点赤经
end
sate_2=[
1  18  6 ; 2  18  42 ; 3  18  78 ; 4  18  114 ; 5  18  150 ;
6  18  -30; 7  18  -66; 8  18  -102; 9  18  -138; 10 18  -174;
11 138 18.2 ; 12 138 54.2 ; 13 138 90.2 ; 14 138 126.2 ; 15 138 162.2 ;
16 138 -17.8; 17 138 -53.8; 18 138 -89.8; 19 138 -125.8; 20 138 -161.8;
21 258 10.4 ; 22 258 46.4 ; 23 258 82.4 ; 24 258 118.4 ; 25 258 154.4 ;
26 258 -25.6; 27 258 -61.6; 28 258 -97.6; 29 258 -133.6; 30 258 -169.6
];
for j=1:30
     sate_2(j,3)=sate_2(j,3)*pi/180;%平近点角
     sate_2(j,2)=sate_2(j,2)*pi/180;%升交点赤经
end
%w=;                    %近地点角
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t_0=0;                %星历的参考历元
t_k=0;                %仿真时间,单位s


a3_1=a_1^3;
a3_2=a_2^3;
n_1=sqrt(mu/a3_1);       % n=(2*pi)/T=sqrt(mu/a3),应用了开普勒第三定律
n_2=sqrt(mu/a3_2);     
k=1;
i=1;
r=1;

%s=43201;
s=86401;    %24h
dot_step=200;
q=0;

while t_k<s     %共计11小时58分
    q=q+1;        %各个矩阵的行数表示量
    t_k
    t(q,1)=t_k;
    i=1;
    sum_s_3(q,1)=0;
    j=1;            % 卫星标号
    while j<=3     %各个矩阵的列数表示量
         %%%%%%计算仰角 E=arctan(Z/sqrt(X^2+Y^2)) ,E_rad单位rad ,E_deg单位度 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
         delta_x(q,j)=X_BD(j,1)-xp;
         delta_y(q,j)=Y_BD(j,1)-yp;
         delta_z(q,j)=Z_BD(j,1)-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_3(q,j)=E_deg(q,j);
         if ele_3(q,j)>=E0
             ele_3(q,j)=1;
             sum_s_3(q,1)=sum_s_3(q,1)+ele_3(q,j);
             if r~=q
                 i=1;
                 r=q;
             end
             % s_n 为 q 时刻(比真实时刻 t_k 加 1 )可见星的标号的矩阵
             s_n_3(q,i)=j;
             i=i+1;
         else
             ele_3(q,j)=0;
         end
         j=j+1;
    end
    %
    i=1;
    j=1;            % 卫星标号
    sum_s_1(q,1)=0;
    while j<=24     %各个矩阵的列数表示量
          M_k(q,j)=sate_1(j,3)+n_1*(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_1*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_1;               %分母一定是是大于0的数,所以只取分子来做判断
          %B=(sin(E_k(q,j))*sqrt(1-e^2))/(1-e*cos(E_k(q,j)));%% f 的正弦
          B=sqrt(1-e_1^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
         % 求偏近点角值\真近点角值\平近点角值相互之间的差值 delta_Ef ，delta_EM，delta_fM %   
                  
         % 计算升交点角距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*(1-e_1*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_gps;    
         % L_k(q,j)=sate_1(j,2)+(dW-w_ie)*(t_k-t_0)-w_ie*t_0;
         L_k(q,j)=sate_1(j,2)+w_ie*(t_k);%%%%%%%%%%%  jia 还是 jian ？？
         % 计算导航星的位置,在地心坐标系中,ECEF(Earth-Centered Earth-Fixed)坐标系 %
         xk_1(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));    
         yk_1(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));
         zk_1(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)=xk_1(q,j)-xp;
         delta_y(q,j)=yk_1(q,j)-yp;
         delta_z(q,j)=zk_1(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_1(q,j)=E_deg(q,j);
         if ele_1(q,j)>=E0
             ele_1(q,j)=1;
             sum_s_1(q,1)=sum_s_1(q,1)+ele_1(q,j);
             if r~=q
                 i=1;
                 r=q;
             end
  % s_n 为 q 时刻(比真实时刻 t_k 加 1 )可见星的标号的矩阵
             s_n_1(q,i)=j;
             i=i+1;
         else
             ele_1(q,j)=0;
         end
         j=j+1;
    end
    %%%%%%%%%%%%%%%%%%
    i=1;
    j=1;            % 卫星标号
    sum_s_2(q,1)=0;
    while j<=30     %各个矩阵的列数表示量
         M_k(q,j)=sate_2(j,3)+n_2*(t_k-t_0);    
         r_k(q,j)=a_2;
         %卫星在轨道直角坐标系中的坐标
         gui_1(q,j)=r_k(q,j)*cos(M_k(q,j));
         gui_2(q,j)=r_k(q,j)*sin(M_k(q,j));
         gui_3(q,j)=0;
         %天球坐标系中坐标
         xk_2(q,j)=gui_1(q,j)*cos(sate_2(j,2))-gui_2(q,j)*sin(sate_2(j,2))*cos(i_0_galileo);
         yk_2(q,j)=gui_1(q,j)*sin(sate_2(j,2))+gui_2(q,j)*cos(sate_2(j,2))*cos(i_0_galileo);
         zk_2(q,j)=gui_2(q,j)*sin(i_0_galileo);
         D_s=1.0e-006 *[0.2378   -0.1026   -0.1235;    0.1026    0.2378    0.0373;    0.1235   -0.0373    0.2378];
         T=[0.3742;   -0.6088;    0.3613];
         X_G=[xk_2(q,j);yk_2(q,j);zk_2(q,j)]+T+D_s*[xk_2(q,j);yk_2(q,j);zk_2(q,j)];
            xk_g(q,j)=X_G(1,1);
            yk_g(q,j)=X_G(2,1);
            zk_g(q,j)=X_G(3,1);
         %%%%%%计算仰角 E=arctan(Z/sqrt(X^2+Y^2)) ,E_rad单位rad ,E_deg单位度 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
         delta_x(q,j)=xk_2(q,j)-xp_2;
         delta_y(q,j)=yk_2(q,j)-yp_2;
         delta_z(q,j)=zk_2(q,j)-zp_2;
         %求卫星在 %%%  站心坐标系下 %%%% 的坐标
         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_2(q,j)=E_deg(q,j);
         if ele_2(q,j)>=E0
             ele_2(q,j)=1;
             sum_s_2(q,1)=sum_s_2(q,1)+ele_2(q,j);
             if r~=q
                 i=1;
                 r=q;
             end
  % s_n 为 q 时刻(比真实时刻 t_k 加 1 )可见星的标号的矩阵
             s_n_2(q,i)=j;
             i=i+1;
         else
             ele_2(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)=sum_s_1(q,1)+sum_s_2(q,1)+sum_s_3(q,1);
end

%figure(3)
%plot3(x_k(:,1),y_k(:,1),z_k(:,1),'b.')
%title('卫星1的空间位置曲线')
%grid;

%figure(4)
%plot(t,E_deg(:,4),'b')
%title('卫星1的高度角仿真')
%grid;
figure(5)
plot(t,sum_s(:,1),'b',t,sum_s(:,1),'b.')
title('可见星数目n仿真')
xlabel('t');ylabel('n')
axis([0,s,0,57])
grid;