uposit.m

来自「GPS TOOLBOX包含以下内容： 1、GPS相关常量和转换因子； 2、角」· M 代码 · 共 69 行

%                                uposit.m
%  Scope:   This MATLAB macro computes GPS user's position by using an 
%           iterative method, when at least four ECEF satellite positions and 
%           the corresponding pseudoranges are known.
%  Usage:   [upos,ucbias] = uposit(svpos,rho,upos)
%           [upos,ucbias] = uposit(svpos,rho,upos,niter)
%  Description of parameters:
%           svpos  -  input, four ECEF satellite positions, each row contains
%                     the three component for a satellite, all components are
%                     in meters
%           rho    -  input, columnwise four pseudoranges, the pseudoranges 
%                     are in meters
%           upos   -  input/output, ECEF user's position, the components are 
%                     in meters; as input, the initialization can be made with
%                     previous computed value 
%           niter  -  input (optional), number of iterations to be executed;
%                     when is not specified only one iteration is executed
%           ucbias -  output, user's clock bias (the difference between user's
%                     time and GPS time) measured in units of distance (meters)
%  Last update:  08/24/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.

function  [upos,ucbias] = uposit(svpos,rho,upos,niter)

if  nargin == 3
   niter = 1;
end   
[nrow,ncol] = size(svpos);
if  (nrow < 4) | (ncol ~= 3)
   error('Error1  -  UPOSIT; check the dimension of svpos');
end
[nrow,ncol] = size(rho);
if  (nrow < 4) | (ncol ~= 1)
   error('Error2  -  UPOSIT; check the dimension of rho');
end

for kk = 1:niter

%  Form the matrix  H

   for k = 1:nrow
      temp(1) = upos(1) - svpos(k,1);
      temp(2) = upos(2) - svpos(k,2);
      temp(3) = upos(3) - svpos(k,3);
      tempt(k) = sqrt(temp(1)*temp(1) + temp(2)*temp(2) + temp(3)*temp(3));  
      for kk = 1:3
         h(k,kk) = temp(kk)/tempt(k);
      end
      h(k,4) = 1.;
   end

%  Form the measurement residuals

   for k = 1:nrow
      measres(k) = rho(k) - tempt(k);
   end

%  Determine delta_x, delta_y, delta_z, and deltat

   deltap = h\measres';

%  Determine a new position fix

   upos(1) = deltap(1) + upos(1);
   upos(2) = deltap(2) + upos(2);
   upos(3) = deltap(3) + upos(3);
   ucbias = deltap(4);

end   %  end of the niter loop