xgcnav.m

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

%                                xgcnav.m
%  Scope:   This MATLAB program determines great circle navigation position
%           velocity, and acceleration when the ECEF initial position and 
%           velocity are specified; WGS-84 constants are used.
%  Usage:   xgcnav
%  Inputs:  - selection of the default input data or enter the following 
%             input data from keyboard:
%             - initial ECEF x-component position, in meters,
%             - initial ECEF y-component position, in meters,
%             - initial ECEF z-component position, in meters,
%             - initial ECEF x-component velocity, in meters/second,
%             - initial ECEF y-component velocity, in meters/second,
%             - initial ECEF z-component velocity, in meters/second,
%             - time step, in seconds,
%             - number of steps
%           - selection of the output data file or exit the results on screen;
%  Outputs: - the recorded output data (at each time step) are as follows:
%             - time, in seconds,
%             - ECEF position, the components in meters,
%             - great circle velocity, the components in meters/second,
%             - great circle acceleration, the components in meters/second^2,
%             on a specified file or on screen
%           - plot of the generated ECEF trajectory in 3D and/or longitude-
%             latitude trajectory (optional)
%  External Matlab macros used:  gcnav, tecefgd, uverv, wgs84con
%  Last update: 05/29/00
%  Copyright (C) 1999-00 by LL Consulting. All Rights Reserved.

clear
close all
yes = 'y';

%  Initialization at the start of great circle dead reckoning    

disp('  ');
answer1 = input('Do you want to use the default data (LAX)? (y/n)[n] ','s');
disp('  ');
if (strcmp(answer1,yes) == 1)
   p0(1) = -2519931.827;      % in meters
   p0(2) = -4659112.609;      % in meters
   p0(3) =  3541187.442;      % in meters
   v0(1) =  150.;             % in meters/second
   v0(2) =  100.;             % in meters/second
   v0(3) =  50.;              % in meters/second
   deltat = 300.;             % in seconds
   nsteps = 10;
else
   p0(1) = input('Specify the initial ECEF x-position, in meters --> ');
   p0(2) = input('Specify the initial ECEF y-position, in meters --> ');
   p0(3) = input('Specify the initial ECEF z-position, in meters --> ');
   v0(1) = input('Specify the initial ECEF x-velocity, in meters/second --> ');
   v0(2) = input('Specify the initial ECEF y-velocity, in meters/second --> ');
   v0(3) = input('Specify the initial ECEF z-velocity, in meters/second --> ');
   deltat = input('Specify the time step, in seconds --> ');
   nsteps = input('Specify the number of steps --> ');
end
pgcm = zeros(nsteps,3);
vgcm = zeros(nsteps,3);
agcm = zeros(nsteps,3);

%   Execute the great circle computation for each time step

[pgcm,vgcm,agcm] = gcnav(p0,v0,deltat,nsteps);

%  Save the generated data into a specified file or displayed on screen

answer2 = input('Do you want to save the generated data? (y/n)[y] ','s');
if  isempty(answer2)
   answer2 = 'y';
end
disp('  ');
if strcmp(answer2,yes) == 1
   f2 = input('Specify the output filename  -->  ','s');
   disp('  ');   
   time = 0.;
   for k = 1:nsteps
      time = time + deltat;
      fprintf(f2,'%10.3f ',time);
      fprintf(f2,' %12.3f %12.3f %12.3f',pgcm(k,1),pgcm(k,2),pgcm(k,3));
      fprintf(f2,' %12.3f %12.3f %12.3f',vgcm(k,1),vgcm(k,2),vgcm(k,3));
      fprintf(f2,' %12.3f %12.3f %12.3f\n',agcm(k,1),agcm(k,2),agcm(k,3));
   end
end

%  Execute the 3-D ECEF trajectory plot (optional)

answer3 = input('Do you want to plot the 3-D trajectory? (y/n)[y] ','s');
if  isempty(answer3)
   answer3 = 'y';
end
disp('  ');
if strcmp(answer3,yes) == 1
   x = zeros(nsteps+1,1);
   y = zeros(nsteps+1,1);
   z = zeros(nsteps+1,1);
   x(2:nsteps+1) = pgcm(:,1);
   y(2:nsteps+1) = pgcm(:,2);
   z(2:nsteps+1) = pgcm(:,3);
   x(1) = p0(1);
   y(1) = p0(2);
   z(1) = p0(3);
   figure(1)
   plot3(x,y,z),grid,...
   xlabel('ECEF x-axis, in meters');
   ylabel('ECEF y-axis, in meters');
   zlabel('ECEF z-axis, in meters');
   aa ='Great circle 3-D ECEF trajectory trajectory (time step = ';
   aa =[aa  num2str(deltat) ' seconds)'] ;
   title(aa);
   hold on
   plot3(x,y,z,'*');
end

%  Execute the 2-D longitude-latitude trajectory plot (optional)

answer4 = input('Do you want to plot the lon-lat trajectory? (y/n)[y] ','s');
if  isempty(answer4)
   answer4 = 'y';
end
disp('  ');
if strcmp(answer4,yes) == 1
   
   xlon = zeros(nsteps+1,1);
   xlat = zeros(nsteps+1,1);
   [lat,lon,alt] = tecefgd(p0);
   xlon(1) = lon;
   ylat(1) = lat;
   for k = 1:nsteps   
      xyz(1) = pgcm(k,1);
      xyz(2) = pgcm(k,2);
      xyz(3) = pgcm(k,3);
      [lat,lon,alt] = tecefgd(xyz);
      xlon(k+1) = lon;
      ylat(k+1) = lat;
   end
   
   figure(2)
   plot(xlon,ylat),grid,...
   xlabel('Longitude, in radians');
   ylabel('Latitude, in radians');
   aa ='Great circle Longitude-Latitude trajectory (time step = ';
   aa = [aa  num2str(deltat) ' seconds)'] ;
   title(aa);
   hold on
   plot(xlon,ylat,'*');
end

disp('End of the program  XGCNAV.M ');
disp('  ');