function [est] = calUsrSGMP(gpsmeas, est);    
%---------------------------------------------------------
% Calculate user position, clk bias based on satellite position
% and pseudo range measurement
% Ju-Yong Do,  Feb 3, 2009
%---------------------------------------------------------
% input  : satellite position, pseudo range measurement
% output : user position, clk bias
%=========================================================
%=========================================================
if ~est.valid, return; end;
global opt const

%=========================================================
%Calculate satellite position w. iterative estimation
estUsr      = [est.oldpos est.oldclkbias]; 
est.valid   = 0;
for i=1:20    
    estPos      = estUsr(1:gpsmeas.dim);        %position est
    estClkBias  = estUsr(gpsmeas.dim+1:end);    %clk bias est

    %Calculate difference in reconstructed pr and measured pr
    estPrVec    = gpsmeas.txpos - ones(gpsmeas.numtx,1)*estPos; %range vector
    estPr       = sqrt(sum(estPrVec(:,1:gpsmeas.dim).^2,2));    %estimated pr
    dPr         = gpsmeas.pr - estPr - estClkBias;              %discrepancy in pr vs. estPr 
    G           = [-((1./estPr)*ones(1,gpsmeas.dim)) .* (gpsmeas.txpos-ones(gpsmeas.numtx,1)*estPos),...
                  ones(gpsmeas.numtx,1)];       %Geometric matrix
        
    %weighting
    dPr         = gpsmeas.weight*dPr;           %weighted pr 
    G           = gpsmeas.weight*G;             %weighted geometric mat
        
    %Reflect pr differenc to position difference
    dUsr        = G\dPr;                        %user position difference
    estUsr      = estUsr + dUsr';               %update pos estimates
    
    %exit if converged
    if all(abs(dUsr) < const.lmt.conv), est.valid = 1; break; end
end

est.pos     = estUsr(1:gpsmeas.dim);            %position est
est.clkbias = estUsr(gpsmeas.dim+1:end);        %clk bias est 

%=========================================================
%Calculate posfix related parameters for record
%DOPs
GEnu        = gpsmeas.weight\ (G*[[est.rotmat zeros(gpsmeas.dim,1)];[zeros(1,gpsmeas.dim) 1]]');    %deweight
hdiag       = diag(pinv(GEnu'*GEnu));           %diagonal component of H matrix for DOP calculation
est.pdop    = sqrt(sum(hdiag(1:3)));            %position dop
est.hdop    = sqrt(sum(hdiag(1:2)));            %horizontal position dop
est.vdop    = sqrt(hdiag(3));                   %vertical position dop
est.tdop    = sqrt(sum(hdiag(4:end)));          %time dop (could be multiple)
est.gdop    = sqrt(sum(hdiag(1:4)));            %geometric dop (position + time)
%others
est.numtx   = gpsmeas.numtx;                    %number of used channels

return;
%=========================================================
%=========================================================
