xopcr.m

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

%                                     xopcr.m
%  Scope:   This program determines operation counts (flops) for four different
%           implementations of RAIM algorithm (only the computation of the
%           decision variable for the RAIM algorithm proposed by Sturza [1]).
%  Usage:   xopcr
%  Inputs:  - name of the input file containing line-of-sight unit vectors, 
%             each vector is stored in another row (it should be 3 columns)
%           - name of the output file containing the results
%           - other parameters are initialized by default
%  Outputs: - table containing the computed operation counts (flops) for all 
%             four different implementations
%  References: 
%           [1] Sturza, M. A., Navigation system integrity monitoring using
%               redundant measurements. Navigation, Journal of the Institute
%               of Navigation, Vol. 35, No. 4, Winter 1988-89, pp. 483-501.
%           [2] 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.
%  Last update: 01/04/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.

clear
yes = 'y';

%  Read the input file containing line-of-sight (LOS) unit vectors

disp(' ');
answer1 = input('Do you want to use the default data? (y/n)[y] --> ','s');
if isempty(answer1)
   answer1 = yes;
end   
if (strcmp(answer1,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 ];
else  
   disp('  ');
   disp('Enter line-of-sight (LOS) unit vectors ');
   ff = input('Specify the input filename (with extension) --> ','s');
   g = load(ff);
end
[nrow,ncol] = size(g);
 
if (ncol~= 3) | (nrow < 5)
   disp('Error -  XOPCR; check the unit line of sight vectors');
   disp(' ');
   return
end

m = nrow;                 %  number of rows (measurements)
n = 4;                    %  number of columns in measurement matrix
unitvec = ones(nrow,1);
h = [g unitvec];          %  form the matrix  H

rand('seed',0.);          %  uniform distribution in the interval  (0,1)
a = 2. * rand(1,nrow) - ones(1,nrow);
                          %  uniform distribution in the interval  (-1,1)
zvec = 33.3 * a';         %  measurement residual (SPS with SA on),in  meters

%  Method 1   --  by using a direct computation method

flops(0)                            %  initialization of the flops counter
hpseudo = (inv(h'* h)) * h';        %  when  h  is full rank
s =  eye(m) -  h * hpseudo;
f = s * zvec;
d1 = f' * f;                        %  decision variable
flops1 = flops;                     %  number of flops

%  Method 2   --  by using a direct computation method when pseudoinverse
%                 is determined by using SVD  (Matlab function)

flops(0)                            %  initialization of the flops counter
hpseudo = pinv(h);                  %  Matlab pseudoinverse
s =  eye(m) -  h * hpseudo;
f = s * zvec;
d2 = f' * f;                        %  decision variable
flops2 = flops;                     %  number of flops

%  Method 3   --  by using the SVD method for decomposition of  H

flops(0)                            %  initialization of the flops counter
[u,sigma,v] = svd(h,0);             %  singular value decomposition
zhat = u' * zvec;
d3 = zvec' * zvec - zhat' * zhat;   %  decision variable
flops3 = flops;                     %  number of flops

%  Method 4   --  by using the QR method for decomposition of  H

flops(0)                            %  initialization of the flops counter
[q,r] = qr(h);                      %  QR  decomposition
np1 = n + 1;                        
qp = q(1:m,np1:m);
ftilde = qp' * zvec;
d4 = ftilde' * ftilde;              %  decision variable         
flops4 = flops;                     %  number of flops

%  Determine the "theoretical" flops for each method [2]

t1 = n * (0.5*m*m + 1.5*m*n + n*n) + m*m + m;
t2 = n * (2.5*m*m + 2.*m*n +11.*n*n) + m*m + m;
t3 = n * (2.*m*m + (19./3.)*n*n) + m*n + m + n;
t4 = n*(2.*m*m - m*n + n*n/3.) + (m + 1)*(m - n);

%  Print the results into the output file or display on sreen

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

fprintf(pf1,'*************************************************************\n');
fprintf(pf1,'\n      COMPARISON OF 4 DIFFERENT RAIM IMPLEMENTATIONS\n');
fprintf(pf1,'        (ONLY THE COMPUTATION OF DECISION VARIABLE)\n\n');
fprintf(pf1,'*************************************************************\n');
fprintf(pf1,'\n  OPERATION COUNTS (FLOPS) for the case with %2.0f measurements\n',m);
fprintf(pf1,'\n*************************************************************');
fprintf(pf1,'\nMethod     Decision Value    Matlab Flops   Theoretical Flops\n');
fprintf(pf1,'*************************************************************\n');
fprintf(pf1,'   1   %20.14f    %6.0f       %8.0f\n',d1,flops1,t1);
fprintf(pf1,'   2   %20.14f    %6.0f       %8.0f\n',d2,flops2,t2);
fprintf(pf1,'   3   %20.14f    %6.0f       %8.0f\n',d3,flops3,t3);
fprintf(pf1,'   4   %20.14f    %6.0f       %8.0f\n',d4,flops4,t4);
fprintf(pf1,'*************************************************************\n');
fprintf(pf1,'where\n');
fprintf(pf1,'method 1 - direct computation implementation\n');
fprintf(pf1,'method 2 - direct computation implementation when the pseudo-\n');
fprintf(pf1,'           inverse is determined by using SVD (Matlab function)\n');
fprintf(pf1,'method 3 - SVD method for measurement matrix decomposition\n');
fprintf(pf1,'method 4 - QR method for measurement matrix decomposition\n');
fprintf(pf1,'*************************************************************\n');
disp(' ');

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