xuposit.m

GPS TOOLBOX包含以下内容： 1、GPS相关常量和转换因子； 2、角度变换； 3、坐标系转换： &#61656 点变换； &#61656 矩阵变换； &#61656 向量变换

%                                xuposit.m
%  Scope:   This MATLAB program computes GPS user's position by using an
%           iterative method, when at least four ECEF satellite positions and 
%           the corresponding pseudoranges are known.
%  Usage:   xuposit
%  Inputs:  - name of the output file if selected, otherwise the data are
%             displayed on screen
%           - name of the ASCII input data file; each row contains the three 
%             components of the ECEF satellite position and the corresponding
%             pseudorange (all values are in meters)
%           - number of iterations to be executed; at least 5 iterations 
%             is recommended 
%           - initial user's position in ECEF; the default is (0., 0., 0.)
%  Outputs: - input data, ECEF position and corresponding pseudorange for
%             each satellite
%           - computed ECEF user's position and computed user's clock bias
%             measurement in units of distance at each iteration
%           - position fix convergence summary; the "true" position fix is 
%             the last computed position fix
%  External Matlab macros used:  uposit
%  Last update:  08/24/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.

clear  
yes = 'y';
answer2 = yes;

%  Specify the output file if selected

disp(' ');
answer1 = input('Do you want to save the results? (y/n)[n] --> ','s');
if isempty(answer1)
   answer1 = 'n';
end   
disp(' ');
if (strcmp(answer1,yes) == 1)
   f2 = input('Specify the output filename (with extension) -->  ','s');
   disp(' ');
else
   f2 = 1;      %  output to the screen
end

while (strcmp(answer2,yes) == 1)
   clear  tt uposstep
   disp('Enter satellite+pseudorange input data - the default is sv7posr.dat');
   answer3 = input('Do you want to use the default datafile? (y/n)[y] ','s');
   if isempty(answer3)
      answer3 = yes;
   end   
   if (strcmp(answer3,yes) == 1)
      f1 = 'sv7posr.dat';
   else
      f1 = input('Specify the input filename (with extension) --> ','s');
   end
   disp('  ');
   niter = input('Specify the number of iterations --> ');
   disp('  ');
   uposstep = zeros(niter+1,3);
   ynorm = zeros(1,niter+1);
   
%  Read the input data file

   tt = load(f1);
   [nrow,ncol] = size(tt);

   if  ( (nrow < 4) | (ncol ~= 4) )
      disp('Error1 - XUPOSIT; check the input data - row/column number');
      disp('  ');
      disp('End of the program  XUPOSIT');
      disp('  ');
      return
   end

   svpos = tt(1:nrow,1:3);
   rho = tt(1:nrow,4);

%  Select position initialization (in ECEF, in meters)

   disp('Do you want to use the default position initialization -');
   answer4 = input('specified by (0.,0.,0.)? (y/n)[y] --> ','s');
   disp('  ');
   if  isempty(answer4)
      answer4 = yes;
   end
   if  (strcmp(answer4,yes) == 1)
      upos(1) = 0.;
      upos(2) = 0.;
      upos(3) = 0.;
   else
      upos(1) = input('Enter data for upos(1) -->  ');
      disp('  ');
      upos(2) = input('Enter data for upos(2) -->  ');
      disp('  ');
      upos(3) = input('Enter data for upos(3) -->  ');
      disp('  ');
   end
   uposstep(1,:) = upos;    %  save the initial position
   
%  Write input data

   fprintf(f2,'********************************  INPUT  DATA  ************');
   fprintf(f2,'********************\n\n');
   for  k = 1:nrow
      fprintf(f2,'For SV # %3.0f \n',k);
      fprintf(f2,'   *****  ECEF position (in meters) :\n');
      fprintf(f2,'%20.7f  %20.7f  %20.7f\n',svpos(k,1),svpos(k,2),svpos(k,3));
      fprintf(f2,'   *****  Pseudorange (in meters) :\n');
      fprintf(f2,'%20.7f\n\n',rho(k));
   end
   fprintf(f2,'Initial user"s ECEF position (in meters) :\n');
   fprintf(f2,'%20.7f  %20.7f  %20.7f\n',upos(1),upos(2),upos(3));
   fprintf(f2,'\n*******************************  RESULTS  *****************');
   fprintf(f2,'********************\n\n');

%  Compute ECEF user's position

   for kk = 1:niter
      [upos,ucbias] = uposit(svpos,rho,upos);
      
      uposstep(kk+1,:) = upos;      % save the position at kk-th iteration

%  Save computed results into an external file

      fprintf(f2,'***** After iteration  = %3.0f \n',kk);
      fprintf(f2,'Computed ECEF user"s position (in meters) :\n');
      fprintf(f2,'%20.7f  %20.7f  %20.7f  \n\n',upos(1),upos(2),upos(3));
      fprintf(f2,'Computed user"s clock bias measured in units of distance ');
      fprintf(f2,' (in meters) :\n %24.14f \n\n',ucbias);
   end
   
   uposf(1:3) = uposstep(niter+1,1:3);
   fprintf(f2,'***** Position fix error convergence - summary\n');
   for kk = 1:niter+1
      temp(1:3) = uposstep(kk,1:3) - uposf(1:3);
      ynorm = norm(temp);
      if kk == 1   
         fprintf(f2,'Initial position fix error = %16.7f meters\n',ynorm);
      else
         fprintf(f2,'At iteration %3.0f Position fix error = %16.7f meters\n',...
                     (kk-1),ynorm);        
      end   
   end         
   
   fprintf(f2,'\n***********************************************************');
   fprintf(f2,'********************\n\n');

%  Select another computation, if desired

   answer2 = input('Do you want another computation? (y/n)[n] --> ','s');
   disp('  ');
   if  isempty(answer2)
      answer2 = 'n';
   end
   
end

disp('End of the program  XUPOSIT');
disp('  ');