single point positioning.asv

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

function SPP(HeadODat,ObsODat)
%多个历元加权平均求测站点坐标
    c=300000000;
    epochNUM=size(ObsODat,2);
    %观测数据的个数
    X=zeros(epochNUM,1);
    Y=zeros(epochNUM,1);
    Z=zeros(epochNUM,1);
    Px=zeros(epochNUM,1);
    Py=zeros(epochNUM,1);
    Pz=zeros(epochNUM,1);
    sigma=zeros(epochNUM,1);
    sigmaX=zeros(epochNUM,1);
    sigmaY=zeros(epochNUM,1);
    sigmaZ=zeros(epochNUM,1);
    for k=1:epochNUM
        Odata=ObsODat(1,k);     
        Xk0=HeadODat.ApproXYZ(1,1);         %测站点的近似坐标
        Yk0=HeadODat.ApproXYZ(1,2);
        Zk0=HeadODat.ApproXYZ(1,3);
      
        tr=ObsODat(1,k).TimeOEpp; 
        %历元接收数据时间
        time=ObsODat(1,k).TimeOEp; 
        Snum=ObsODat(1,k).SatSum;        %该历元观测到的卫星数
     
        Code=ObsODat(1,k).SatCode;       %该历元观测到的卫星号组
        R=ObsODat(1,k).Obs_RangeC1';      %取出C1观测值,列向量
        
        
        %卫星发射时间的迭代计算
        x=10000*ones(4,1);
        row=0;
        while (abs(x(1,1))>10 | abs(x(2,1))>10 | abs(x(3,1))>10 | abs(x(4,1))>10) & row<100
            XYZs=zeros(3,Snum);
            Dis=zeros(Snum,1);
            dts=zeros(Snum,1);                        %卫星钟差
            for t=1:Snum
                n=Code(t);
                ts0=tr-0.075;      %ts为GPSsec0nd
                ts1=tr;
                line=0;
                while abs(ts1-ts0)>=0.00000001
                    if line==1
                        ts0=ts1;
                    end
                    line=1;
                    Scoordinate=CalPos(n,time);
                    Xs=Scoordinate(1,1);
                    Ys=Scoordinate(1,2);
                    Zs=Scoordinate(1,3);
                    Dis0=sqrt((Xk0-Xs)^2+(Yk0-Ys)^2+(Zk0-Zs)^2);
                    ts1=tr-Dis0/c;
                end
                 XYZs(:,t)=Scoordinate';
                Xs=XYZs(1,t);
                Ys=XYZs(2,t);
                Zs=XYZs(3,t);
                Dis(t,1)=sqrt((Xk0-Xs)^2+(Yk0-Ys)^2+(Zk0-Zs)^2);
                dts(t,1)=dt;
                %dts(t,1)=dt+dt0;
            end
            XYZs=XYZs';
    
            %构造误差方程
            A=zeros(Snum,3);    %系数项
            L=zeros(Snum,1);    %常数项
            for t=1:Snum
                A(t,:)=(XYZs(t,:)-HeadODat.ApproXYZ)/Dis(t,1);
                L(t,1)=R(t,1)-Dis(t,1)+c*dts(t,1);
            end
            one=ones(Snum,1);
            A=[-A,one];
    
            %构造法方程并求解
            if Snum>4 
                N=A'*A;
                Q=inv(N);
                U=A'*L;
                x=Q*U;
                V=A*x-L;
                sigma0=sqrt(V'*V/(Snum-4));
            elseif Snum==4
                x=inv(A)*L;
                sigma0=0;
                Q=inv(A);
            end
            XYZk=[Xk0;Yk0;Zk0;0];
            XYZk=XYZk+x;
            Xk0=XYZk(1,1)
            Yk0=XYZk(2,1)
            Zk0=XYZk(3,1)
            %dt0=XYZk(4,1);
            row=row+1;
        end
        
        
        P=XYZk;
        Q=diag(Q);
%==========================================================================
        X(k,1)=XYZk(1,1);
        Y(k,1)=XYZk(2,1);
        Z(k,1)=XYZk(3,1);
        Px(k,1)=1/Q(1,1);
        Py(k,1)=1/Q(2,1);
        Pz(k,1)=1/Q(3,1);
        sigma(k,1)=sigma0;
   end
   avrX=sum(X.*Px)/sum(Px);
   avrY=sum(Y.*Py)/sum(Py);
   avrZ=sum(Z.*Pz)/sum(Pz);
   sigmaX=sqrt(sum(sigma.^2.*Px))/sum(Px);
   sigmaY=sqrt(sum(sigma.^2.*Py))/sum(Py);
   sigmaZ=sqrt(sum(sigma.^2.*Pz))/sum(Pz);
   
   avrXYZ=[avrX;avrY;avrZ];
   sigmaXYZ=[sigmaX;sigmaY;sigmaZ];
   return