dddw.m

二． 单点定位 包含五个M文件

function DDDW
clear all
tic
  global HeadODat;
  global ObsODat; 
  global EphDat;
%先读N文件，再读O文件
EphDat=ReadGpsData;
[HeadODat,ObsODat]=ReadObsData;

%多个历元加权平均求测站点坐标

N = size(EphDat,2);
c=299792458;
epochNUM=size(ObsODat,2);
%观测数据的个数

Xk0=HeadODat.ApproXYZ(1,1); %测站点的近似坐标
Yk0=HeadODat.ApproXYZ(1,2);
Zk0=HeadODat.ApproXYZ(1,3);
for k=1:epochNUM
 
    tr=ObsODat(k).TimeOEp; 
    %历元接收数据时间
    Snum=ObsODat(1,k).SatSum;      %该历元观测到的卫星数  
     if Snum<4 
  
            break;       %去除无解的历元
      end
      
    Code=ObsODat(k).SatCode; 
    %该历元观测到的卫星号组

    R=ObsODat(k).Obs_RangeC1 ;  %取出C1观测值,列向量       
    
    %卫星发射时间的迭代计算
  
   
    for j=1:Snum    
        if R(j) == 0
            continue;
        end
        t=TimetoJD(tr(1),tr(2),tr(3),tr(4),tr(5),tr(6));
        t2 = mod(t,7)*24*3600;%gps second
        %卫星钟差
        dd=-1;
        for i=1:N   
            if(EphDat(i).PRN==Code(j)&abs(t-EphDat(i).toc)<0.0417*2)%读入时间相近的星历文件
                a0= EphDat(i).a0; 
                a1=EphDat(i).a1;
                a2= EphDat(i).a2;
                toe=EphDat(i).toe;
                dt=a0+(t2-toe)*a1+(t2-toe)^2*a2;%卫星钟差
                dd=1;
                break;         
            end  
            
        end
        if dd==-1
          msgbox('没有相关星历文件');
           return;
        end
        tt = tr;
        ts = tr(6) - R(j)/c;%用秒进行迭代
        sign = 1;
        while(sign>1E-8)
            tt(6) = ts;
            [Xs1,Ys1,Zs1]= CalPos(Code(j),tt);
             ss1 = sqrt((Xk0-Xs1)^2+(Yk0-Ys1)^2+(Zk0-Zs1)*(Zk0-Zs1));
              %卫星位置加地球自转改正
               qe=0.00007292115;   %地球自转角速度          
               delt=ss1/c;
               
                Rotation=[cos(qe*delt),sin(qe*delt),0;-sin(qe*delt),cos(qe*delt),0;0,0,1];        
                h=Rotation*[Xs1;Ys1;Zs1] ;
                 Xs=h(1);
                 Ys=h(2);
                 Zs=h(3);
            ss = sqrt((Xk0-Xs)^2+(Yk0-Ys)^2+(Zk0-Zs)*(Zk0-Zs));
            ts = tr(6)-ss/c;
            sign = norm(ts-tt(6));
        end
          
       
        axk = (Xk0-Xs)/ss;
        ayk = (Yk0-Ys)/ss;
        azk = (Zk0-Zs)/ss;
        if j==1
            A = [axk,ayk,azk,1];
            L = R(j)-ss++c*dt;
        else
            A = [A;axk,ayk,azk,1]; %构造误差方程
            L = [L;(R(j)-ss++c*dt)];%常数项
        end
    end
    if  Snum==4
        dx=inv(A)*L;
        aaaa(k).Q=inv(A'*A);
        Px(k) = 1/aaaa(k).Q(1,1);
        Py(k) = 1/aaaa(k).Q(2,2);
        Pz(k) = 1/aaaa(k).Q(3,3);
        xchange(k) = dx(1);
        ychange(k) = dx(2);
        zchange(k) = dx(3);
    elseif  Snum > 4
        dx = inv(A'*A)*A'*L; %构造法方程并求解
        aaaa(k).Q=inv(A'*A);
        Px(k) = 1/aaaa(k).Q(1,1);
        Py(k) = 1/aaaa(k).Q(2,2);
        Pz(k) = 1/aaaa(k).Q(3,3);
        xchange(k) = dx(1);
        ychange(k) = dx(2);
        zchange(k) = dx(3);

    end
end


Xp = sum(Px.*(Xk0+xchange))/sum(Px)
Yp = sum(Py.*(Yk0+ychange))/sum(Py)
Zp = sum(Pz.*(Zk0+zchange))/sum(Pz)

figure(1);
subplot(3,1,1);plot(xchange,'b--');
subplot(3,1,2);plot(ychange,'r--');
subplot(3,1,3);plot(zchange,'g--');
toc