dgps_phase.m

利用gps多天线载波相位技术

%%========================================
%%     Toolbox for attitude determination
%%     Zhen Dai
%%     [e-mail]
%%     ZESS, University of Siegen, Germany
%%     Last Modified  : 1.Sep.2008
%%========================================
%% Functions:
%%     Least-squares adjustment for differential positioning using carrier
%%     phase and resolved ambiguities.
%% Input parameters:
%%      mSatPosMaster-> Satellites coordinates of the master antenna
%%      mSatPosSlave-> Satellites coordinates of the slave antenna
%%      vPhaseMaster-> Phase data of the master antenna in cycles
%%      vPhaseSlave->  Phase data of the slave antenna in cycles
%%      key_sat_id->  PRN of the key satellite
%%      vN -> Integer phase ambiguities in the unit of cycles.
%%      vSatId-> Common satellite PRNs
%%      vMasterXYZ -> Estimated value of the master antenna in ECEF
%% Output:
%%      vPos -> Estimated coordinate of the slave antenna in ECEF
%% Remarks:
%%      1. vMasterXYZ can be acquired from the surveying in advance, or
%%          from the single point positioning. Although the latter one is
%%          always of low accuracy, but it is always the case in the attitude
%%          determination when no ground base station avaliable and it will
%%          not significantly degrade the accuracy of the baseline vector. 
%%      2. All the measurement are equally weighted.
%%      3. Key satellite is the satellite having the highest elevation
%%          angle with respect to the master antenna.
%%      4. About the format of vN: the double differenced ambiguity vector is 
%%          constructed based on the same order of mPAB (double differenced 
%%          phase data)given below. vN must be identified before invoking this function.
%%  References:
%%       B.Hofmann-Wellenhof, H.Lichtenegger and J.Collins: GPS Theory
%%       and practice. 2001. Fifth revised edition. Springer, Wien, New York.
%%       pp. 262-264. 

function vPos=DGPS_phase(mSatPosMaster,mSatPosSlave,vPhaseMaster,...
    vPhaseSlave,key_sat_id,vSatId,vN,vMasterXYZ)

%% Format the input parameters
[a,b]=size(vMasterXYZ);
if (a==3) && (b==1),
    vMasterXYZ=vMasterXYZ';
end

sol=299792458; %% Speed of light
freq1=1575.42e6; %%frequency of L1
lambda1=sol/(freq1);%% lambda on L1

%% Initial guess of the starting point for the adjustment
B0=vMasterXYZ;
%% Number of iterations
iteration=0;
while (1)
    iteration=iteration+1;
    %% From Eq. (9.138), In order to get LAB, we need
    %% rou_B0j,rou_B0k,rou_Aj ,rou_Ak
    %% Here j is the key satellite

    %% Calculate rou_B0j rou_Aj
    rou_B0j=norm(mSatPosMaster(key_sat_id,1:3)-B0(1:3));
    rou_Aj=norm(mSatPosMaster(key_sat_id,1:3)-vMasterXYZ(1:3));
    %% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    %%          Construct the required parameters 
    %%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    clear rou_B0k aXB aYB aZB PAB rou_Ak LAB
    m=0;
    for k=1:1:length(vSatId),
        if (vSatId(k)~=key_sat_id),
            m=m+1;
            %%Calculate rou_B0k  rou_Ak
            rou_B0k(m)=norm(mSatPosSlave(vSatId(k),1:3)-B0(1:3));
            rou_Ak(m)=norm(mSatPosMaster(vSatId(k),1:3)-vMasterXYZ(1:3));
            %% Calculate the aXB aYB aZB
            aXB(m)=-(mSatPosSlave(vSatId(k),1)-B0(1))/rou_B0k(m)+(mSatPosSlave(key_sat_id,1)-B0(1))/rou_B0j;
            aYB(m)=-(mSatPosSlave(vSatId(k),2)-B0(2))/rou_B0k(m)+(mSatPosSlave(key_sat_id,2)-B0(2))/rou_B0j;
            aZB(m)=-(mSatPosSlave(vSatId(k),3)-B0(3))/rou_B0k(m)+(mSatPosSlave(key_sat_id,3)-B0(3))/rou_B0j;
            %% Double differenced phase data minus the differenced
            %% ambiguities. Note that both are given in cycles!!!
            PAB(m)=(vPhaseSlave(vSatId(k))-vPhaseSlave(key_sat_id)-vPhaseMaster(vSatId(k))...
                            +vPhaseMaster(key_sat_id)-vN(m))*lambda1;
            %%Now get L_ABjk
            LAB(m)=PAB(m)-rou_B0k(m)+rou_B0j+rou_Ak(m)-rou_Aj;
        end
    end
    %% Weight (euqally weighted)
    C=eye(length(vSatId)-1);
    %% Form A and l in model (9.139 )
    A=[aXB' aYB' aZB'];
    l=LAB';
    %% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    %%          Implement the adjustment and update
    %%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    %% Solution with weighted LSQ
    DeltaXYZ=inv(A'*C*A)*A'*C*l;
    %%Update the solution
    B0=B0+DeltaXYZ';
    %% Check whether the LSQ iteration should be terminated
    if iteration>7 ||  (norm(DeltaXYZ)<1e-7),
        vPos=B0;
        break;
    end
end