xqrupa.m

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

%                               xqrupa.m
%  Scope:  This MATLAB program tests the updating algorithm [1] for Q-R 
%          factorization of the modified measurement matrix when a new 
%          clock measurement is added. 
%  Usage:  xqrupa
%  Inputs:  - name of the output file if selected (the default is the screen)
%           - line-of-sight for the satellites used, entered from a specified
%             file or the specified default values 
%           - ratio between pseudorange measurements standard deviation and
%             clock standard deviation (denoted by alpha), entered from a
%             specified file or the specified default values 
%  Outputs: - input/output data stored into the specified output file or
%             displayed on screen; operation count summary for two methods 
%             (direct Q-R refactorization and updating Q-R factorization)is 
%             included
%  Reference:
%          [1] Copps, E. M., Lupash, L., Extending the generality and
%              robustness of RAIM algorithms. Proceedings of the 1994 National
%              Technical Meeting, Institute of Navigation, San Diego, CA, Jan. 
%              24-26, 1994, pp. 31-39.
%  External Matlab macros used:  qrupa 
%  Last update:  07/31/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.

clear
yes = 'y';

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  -->  ','s');
   disp('  ');
else
   f2 = 1;      %  output to the screen
end

answer2 = input('Do you want to use the default data? (y/n)[y] --> ','s');
if isempty(answer2)
   answer2 = yes;
end   
disp('  ');
if (strcmp(answer2,yes) == 1)
   g  = [ 7.787995e-002   -6.017930e-001    7.947552e-001
          1.730016e-001   -9.665970e-001   -1.891052e-001
         -8.457427e-001   -4.893909e-001   -2.126402e-001
         -5.385451e-001   -2.338803e-001    8.094870e-001
          4.345836e-001   -7.366578e-001   -5.181432e-001
          7.052152e-001   -5.433383e-001    4.554723e-001
         -8.973768e-001    3.366878e-001    2.852302e-001 ];
   alpha = 8.;    %  ratio between pseudorange measurement st. dev. and
                  %  clock st. dev.
   m = 7;         %  number of original measurements -  see matrix g
   ncol = 3;      %  number of columns -  see matrix  g        
else
   disp('Read the line-of-sight for the satellites used ');
   f1 = input('Specify the input filename (with extension) --> ','s');
   disp('  ');
   g = load(f1);
   [m,ncol] = size(g);
   alpha = input('Enter the value of alpha --> ');
   disp('  ');
end

if  ((ncol ~= 3) | (m < 5))
   disp('Error - XQRUPA; check the input data file ');
end   
mp1 = m + 1;
unitvec = ones(m,1);
h = [g unitvec];         %   form the measurement matrix  H
n = 4;

%  Computation of the Q-R factors of the original measurement matrix  H

[q,r] = qr(h);           %   Q-R factorization of the matrix  H

for kk = 1:n
   if r(kk,kk) < 0.
      for kkk = kk:n
         r(kk,kkk) = - r(kk,kkk);
      end
      for kkk = 1:m
         q(kkk,kk) = - q(kkk,kk);
      end
   end
end

%  Save or display the input data

fprintf(f2,'**************************************************************\n'); 
fprintf(f2,'\n   *****   Input data   *****  \n\n');
fprintf(f2,'     H  matrix (input)  \n');
for k = 1:m
   for kk = 1:n
      if rem(kk,9) == 0
         fprintf(f2,'\n'); 
      end
      fprintf(f2,'%7.4f ',h(k,kk)); 
   end
   fprintf(f2,'\n'); 
end
fprintf(f2,'\n     Q  matrix (input)  \n');
for k = 1:m
   for kk = 1:m
      if rem(kk,9) == 0
         fprintf(f2,'\n'); 
      end
      fprintf(f2,'%7.4f ',q(k,kk)); 
   end
   fprintf(f2,'\n'); 
end
fprintf(f2,'\n     R  matrix (input)  \n');
for k = 1:m
   for kk = 1:n
      if rem(kk,9) == 0
         fprintf(f2,'\n'); 
      end
      fprintf(f2,'%7.4f ',r(k,kk)); 
   end
   fprintf(f2,'\n'); 
end
fprintf(f2,'\n     Alpha = %8.4e \n\n',alpha);

res1 = norm(q*r - h);
fprintf(f2,'     Residual = norm(Q*R - H) = %9.5e \n\n',res1);

%  Direct computation of the Q-R factors for the modified measurement matrix

flops(0);                %   initialization of the flops counter
hmp1 = [ 0. 0. 0. alpha];
hh = [h' hmp1']';
[qq,rr] = qr(hh);
for kk = 1:n
   if rr(kk,kk) < 0.
      for kkk = kk:n
         rr(kk,kkk) = - rr(kk,kkk);
      end
      for kkk = 1:mp1
         qq(kkk,kk) = - qq(kkk,kk);
      end
   end
end
flops1 = flops;          %   number of flops

fprintf(f2,'   *****   Output data by using the direct method  *****  \n\n');
fprintf(f2,'     Hupdate  matrix (output)  \n');
for k = 1:mp1
   for kk = 1:n
      if rem(kk,9) == 0
         fprintf(f2,'\n'); 
      end
      fprintf(f2,'%7.4f ',hh(k,kk)); 
   end
   fprintf(f2,'\n'); 
end
fprintf(f2,'\n     Qupdate  matrix (output)  \n');
for k = 1:mp1
   for kk = 1:mp1
      if rem(kk,9) == 0
         fprintf(f2,'\n'); 
      end
      fprintf(f2,'%7.4f ',qq(k,kk)); 
   end
   fprintf(f2,'\n'); 
end
fprintf(f2,'\n     Rupdate  matrix (output)  \n');
for k = 1:mp1
   for kk = 1:n
      if rem(kk,9) == 0
         fprintf(f2,'\n'); 
      end
      fprintf(f2,'%7.4f ',rr(k,kk)); 
   end
   fprintf(f2,'\n'); 
end

res2 = norm(qq*rr - hh);
fprintf(f2,'\n     Residual = norm(Qupdate*Rupdate - Hupdate) = %9.5e\n',res2);

%  Updating the Q-R factorization of the measurement matrix  H  by using
%  macro  qrupa

flops(0);                %   initialization of the flops counter
[qupdate,rupdate] = qrupa(m,n,q,r,alpha);
flops2 = flops;          %   number of flops

fprintf(f2,'\n   *****   Output data by using macro  qrupa  *****  \n\n');
fprintf(f2,'     Qupdate  matrix (output)  \n');
for k = 1:mp1
   for kk = 1:mp1
      if rem(kk,9) == 0
         fprintf(f2,'\n'); 
      end
      fprintf(f2,'%7.4f ',qupdate(k,kk)); 
   end
   fprintf(f2,'\n'); 
end
fprintf(f2,'\n     Rupdate  matrix (output)  \n');
for k = 1:mp1
   for kk = 1:n
      if rem(kk,9) == 0
         fprintf(f2,'\n'); 
      end
      fprintf(f2,'%7.4f ',rupdate(k,kk)); 
   end
   fprintf(f2,'\n'); 
end

res3 = norm(qupdate * rupdate - hh);
fprintf(f2,'\n     Residual = norm(Qupdate*Rupdate - Hupdate) = %9.5e\n',res3);

fprintf(f2,'\n');
fprintf(f2,'**************************************************************\n'); 
fprintf(f2,'\n     OPERATION COUNT (FLOPS) SUMMARY (with %2.0f measurements)',m);
fprintf(f2,'\n               when adding a clock measurement\n');
fprintf(f2,'\n   Method        Description                  Flops  \n');
fprintf(f2,'      1       Direct Q-R refactorization    %6.0f  \n',flops1);
fprintf(f2,'      2       Updating Q-R factorization    %6.0f  \n\n',flops2);
fprintf(f2,'**************************************************************\n'); 
fprintf(f2,'\n');

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