singlepointgps.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 single point positioning
%% Input parameters:
%%      vCode -> Code data
%%      mSatPos-> Satellite positions
%%      vSatID-> Visible satellite PRNS
%%      vCodeSlave->  Code data of the slave antenna
%%      vInitPos->  Starting point of the adjustment
%% Output:
%%      vPos -> Estimated coordinate of the antenna in ECEF
%%      clock_err -> Estimated receiver clock error in seconds
%% Remarks:
%%      All the measurement are equally weighted.
%%  References:
%%      B.Hofmann-Wellenhof, H.Lichtenegger and J.Collins: GPS Theory
%%      and practice. 2001. Fifth revised edition. Springer, Wien, New York.
%%      pp. 256-259.

 
function [vPos,clock_err]=SinglePointGPS(vCode,mSatPos,vSatID,vInitPos)

if nargin<4, 
     vInitPos=[0 0 0];
end
%% Format the input
[a,b]=size(vInitPos);
if (a==3) && (b==1),
    vInitPos=vInitPos';
end

sol=299792458; %% Speed of light
iteration=0;
%% Initialization 
vPos=vInitPos;
clock_err_meter=0;
%% Start the adjsutment
while ((iteration<10)),
    for satid = 1:1:length(vSatID),
        %% Construct the matrix A and L
        vRou0(satid)=norm(mSatPos(vSatID(satid),:)-vPos);
        vL(satid,1) = vCode(vSatID(satid)) -  vRou0(satid) - clock_err_meter;
        for j=1:1:3,
            A(satid,j)=-(mSatPos(vSatID(satid),j)-vPos(j))/vRou0(satid);
        end
        A(satid,4)=1;
    end
    %% Get the update
    vDeltaXYZt=inv(A'*A)*A'*vL;
    %% Exit of the adjustment
    if (norm(vDeltaXYZt(1:3))<1e-7),
        break; 
    end
    %% Adjustment
    vPos=vPos+vDeltaXYZt(1:3)';
    clock_err_meter=clock_err_meter+vDeltaXYZt(4);
    iteration=iteration+1;
end
clock_err=clock_err_meter/sol; %% Convert from meter to second