gps_9.m

本程序设计了读取RINEX格式文件进行单点定位和基线解算算法实现。

%%%%%%%%%%%%%%%%%% gps constellation emluator %%%%%%%%%%%
%--------------------by yujiejie---------------------%
clear
% constant defination%                 
pi=3.1415926;
a=26609e3;            %轨道长半轴长，单位已经换算为 m
e=0.006;              %轨道的偏心率
i_0=55*pi/180;          %基准时间t_0的轨道倾角

a_e=6378137;          %地球椭球的长半径
e_e=1/298.257223563;  %地球椭球的第一偏心率

%测站在大地坐标系中的经纬度数据,经度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);
%求系数阵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
% 卫星轨道参数矩阵，第一列卫星标号1~24，第二列升交点赤经W_0，第三列平近点角距M_0 %
sate=[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];
%w=;                    %近地点角
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t_0=0;                %星历的参考历元
t_k=0;                %仿真时间,单位s


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

for j=1:24
     sate(j,3)=sate(j,3)*pi/180;%平近点角
     %M_k(1,j)=sate(j,3);
     sate(j,2)=sate(j,2)*pi/180;%升交点赤经
end
s=86401;
dot_step=300;
q=0;
%while t_k<43200//43080
while t_k<s     %共计11小时58分
    q=q+1        %各个矩阵的行数表示量
    %t_k
    t(q,1)=t_k;
    j=1;            % 卫星标号
    while j<=24     %各个矩阵的列数表示量
          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
         % 求偏近点角值\真近点角值\平近点角值相互之间的差值 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-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)+w_ie*(t_k);%%%%%%%%%%%  jia 还是 jian ？？
         % 计算导航星的位置,在地心坐标系中,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:24
        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矩阵为可见星标号阵
            %% 以下在站心坐标系下计算 伪距 rou %   
            rou(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)/rou(q,i);a_12(i,q)=(y_k(q,sd(i))-yp)/rou(q,i);
            a_13(i,q)=(z_k(q,sd(i))-zp)/rou(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)];
            rou(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')
    axis([0,s,0,30])
    grid;
fh_pdop=figure('Name','PDOP Value','NumberTitle','off');
    plot(t,PDOP(1,:),'m.');
    title('PDOP值仿真')
    xlabel('t(sec)');ylabel('PDOP')
    axis([0,s,0,15])
    grid;