xmtransf.m

来自「GPS software toolbox for GPS receiver de」· M 代码 · 共 393 行 · 第 1/2 页

%                             xmtransf.m
%  Scope:   This MATLAB program constructs the following coordinate transforma-
%           tion matrices:
%            1) from ECEF to ENU coordinates
%            2) from ENU to ECEF coordinates
%            3) from ECEF to LLWA coordinates
%            4) from LLWA to ECEF coordinates
%            5) from ECEF to INS coordinates
%            6) from INS to ECEF coordinates
%            7) from ENU to LLWA coordinates
%            8) from LLWA to ENU coordinates
%            9) from LLWA to GPS body coordinates
%           10) from GPS body to LLWA coordinates
%           11) from ECEF to ECI coordinates
%           12) from ECI to ECEF coordinates.
%           WGS-84 constants are used.
%  Usage:   xmtransf
%  Inputs:  - name of the output file if selected (the default is the screen)
%           - input data from the keyboard as follows:
%            1) For the ECEF/ENU to ENU/ECEF transformation:
%               - latitude, in radians
%               - latitude, in radians
%            2) For the ECEF/LLWA to LLWA/ECEF transformation:
%               - latitude, in radians 
%               - longitude, in radians 
%               - wander azimuth angle, in radians
%            3) For the ECEF/INS to INS/ECEF transformation:
%               - latitude, in radians 
%               - longitude, in radians
%               - wander azimuth angle, in radians
%            4) For the ENU/LLWA to LLWA/ENU transformation:
%               - wander azimuth angle, in radians
%            5) For the LLWA/GPS Body to GPS Body/LLWA transformation:
%               - yaw angle, in radians 
%               - pitch angle, in radians
%               - roll angle, in radians
%            6) For the ECEF/ECI to ECI/ECEF transformation
%               - time elapsed since reference time, in seconds
%  Outputs:  - input/output data stored into the specified output file or
%              displayed on screen
%  External Matlab macros used: mecefenu, menuecef, mecefllw, mllwecef,
%            mecefins, minsecef, menullw, mllwenu, mllwb, mbllw,
%            mecefeci, meciecef, wgs84con
%  Last update: 04/05/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.

clear  

yes = 'y';
answer = 'y';

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

moutput = zeros(3,3);

while (strcmp(answer,yes) == 1)     

   disp('   Select :  1  -->  ECEF to ENU coordinates');
   disp('             2  -->  ENU to ECEF coordinates');
   disp('             3  -->  ECEF to LLWA coordinates');
   disp('             4  -->  LLWA to ECEF coordinates');
   disp('             5  -->  ECEF to INS coordinates');
   disp('             6  -->  INS to ECEF coordinates');
   disp('             7  -->  ENU to LLWA coordinates');
   disp('             8  -->  LLWA to ENU coordinates');
   disp('             9  -->  LLWA to GPS body coordinates');
   disp('            10  -->  GPS body to LLWA coordinates');
   disp('            11  -->  ECEF to ECI coordinates');
   disp('            12  -->  ECI to ECEF coordinates');
   disp('  ');
   select = input('Make the selection -->  ');
   disp('  ');
   
   if (select == 1)                         % ECEF to ENU Transformation

      lat = input('Enter latitude  in radians --> ');   
      lon = input('Enter longitude in radians --> ');   

%  Determine transformation matrix

      moutput = mecefenu(lat,lon);

%  Save the results if the output file is specified or display on screen

      fprintf(f2,'\n**************************************************');
      fprintf(f2,'******************************\n');
      fprintf(f2,'\n*****   Input data   *****\n');
      fprintf(f2,'      latitude  (rad.)  = %20.12f\n',lat);
      fprintf(f2,'      longitude (rad.)  = %20.12f\n',lon);
      fprintf(f2,'\n*****   ECEF to ENU transformation matrix   *****\n');
      for k = 1:3
         fprintf(f2,'%20.7f  %20.7f  %20.7f\n',...
                     moutput(k,1),moutput(k,2),moutput(k,3));
      end
      fprintf(f2,'\n**************************************************');
      fprintf(f2,'******************************\n');

   elseif (select == 2)                     % ENU to ECEF Transformation

      lat = input('Enter latitude  in radians --> ');   
      lon = input('Enter longitude in radians --> ');   

%  Determine transformation matrix

      moutput = menuecef(lat,lon);

%  Save the results if the output file is specified or display on screen

      fprintf(f2,'\n**************************************************');
      fprintf(f2,'******************************\n');
      fprintf(f2,'\n*****   Input data   *****\n');
      fprintf(f2,'      latitude  (rad.)  = %20.12f\n',lat);
      fprintf(f2,'      longitude (rad.)  = %20.12f\n',lon);
      fprintf(f2,'\n*****   ENU to ECEF transformation matrix   *****\n');
      for k = 1:3
         fprintf(f2,'%20.7f  %20.7f  %20.7f\n',...
                     moutput(k,1),moutput(k,2),moutput(k,3));
      end
      fprintf(f2,'\n**************************************************');
      fprintf(f2,'******************************\n');

   elseif (select == 3)                     % ECEF to LLWA Transformation

      lat = input('Enter latitude  in radians      --> ');   
      lon = input('Enter longitude in radians      --> ');   
      waz = input('Wander azimuth angle in radians --> ');

%  Determine transformation matrix

      moutput = mecefllw(lat,lon,waz);

%  Save the results if the output file is specified or display on screen

      fprintf(f2,'\n**************************************************');
      fprintf(f2,'******************************\n');
      fprintf(f2,'\n*****   Input data   *****\n');
      fprintf(f2,'      latitude  (rad.)       = %20.12f\n',lat);
      fprintf(f2,'      longitude (rad.)       = %20.12f\n',lon);
      fprintf(f2,'      wander azimuth (rad.)  = %20.12f\n',waz);
      fprintf(f2,'\n*****   ECEF to LLWA transformation matrix   *****\n');
      for k = 1:3
         fprintf(f2,'%20.7f  %20.7f  %20.7f\n',...
                     moutput(k,1),moutput(k,2),moutput(k,3));
      end
      fprintf(f2,'\n**************************************************');
      fprintf(f2,'******************************\n');

   elseif (select == 4)                     % LLWA to ECEF Transformation

      lat = input('Enter latitude  in radians      --> ');   
      lon = input('Enter longitude in radians      --> ');   
      waz = input('Wander azimuth angle in radians --> ');

%  Determine transformation matrix

      moutput = mllwecef(lat,lon,waz);

%  Save the results if the output file is specified or display on screen

      fprintf(f2,'\n**************************************************');
      fprintf(f2,'******************************\n');
      fprintf(f2,'\n*****   Input data   *****\n');
      fprintf(f2,'      latitude  (rad.)       = %20.12f\n',lat);
      fprintf(f2,'      longitude (rad.)       = %20.12f\n',lon);
      fprintf(f2,'      wander azimuth (rad.)  = %20.12f\n',waz);
      fprintf(f2,'\n*****   LLWA to ECEF transformation matrix   *****\n');
      for k = 1:3
         fprintf(f2,'%20.7f  %20.7f  %20.7f\n',...
                     moutput(k,1),moutput(k,2),moutput(k,3));
      end
      fprintf(f2,'\n**************************************************');
      fprintf(f2,'******************************\n');

   elseif (select == 5)                     % ECEF to INS Transformation

      lat = input('Enter latitude  in radians      --> ');   
      lon = input('Enter longitude in radians      --> ');   
      waz = input('Wander azimuth angle in radians --> ');

%  Determine transformation matrix

      moutput = mecefins(lat,lon,waz);

%  Save the results if the output file is specified or display on screen

      fprintf(f2,'\n**************************************************');
      fprintf(f2,'******************************\n');
      fprintf(f2,'\n*****   Input data   *****\n');
      fprintf(f2,'      latitude  (rad.)       = %20.12f\n',lat);