xtrajs.m

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

%                                xtrajs.m
%  Scope:   This MATLAB program determines the vehicle trajectory with the 
%           following characteristics: straight segment with constant speed
%           in ENU frame; the trajectory is specified in ECEF and ENU frames.
%           WGS-84 constants are used.
%  Usage:   xtrajs
%  Inputs:  - selection of the default input data or enter the following 
%             input data from keyboard:
%             - initial ECEF position, the components in meters
%             - ENU velocity (constant), the components 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:
%             - trajecef, ECEF position, the components are in meters;
%               trajecef(k+1,:) is the k-th time step value
%             - trajenu , ENU position, the components are in meters;
%               trajenu(k+1,:) is the k-th time step value; the origin is the
%               initial user's position
%             on a specified file or on screen
%           - plots of the generated 3-D ECEF trajectory, 2-D ENU trajectory, 
%             and  longitude-latitude trajectoru (optional).
%  External Matlab macros used:  tecefgd, trajs, venuecef, 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 the trajectory generation    

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

%   Execute the trajectory generation for each time step

 [trajecef,trajenu] = trajs(user0xyz,v0enu,deltat,nsteps);

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

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

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

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

answer4 = input('Do you want to plot the 3-D ECEF trajectory? (y/n)[y] ','s');
if  isempty(answer4)
   answer4 = 'y';
end
disp('  ');
if strcmp(answer4,yes) == 1
   x = trajecef(:,1);
   y = trajecef(:,2);
   z = trajecef(:,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');
   title('3-D ECEF trajectory');
   hold on
   plot3(x,y,z,'*');
end

%  Execute the ENU x-y trajectory plot (optional)

answer5 = input('Do you want to plot the ENU x-y trajectory? (y/n)[y] ','s');
if  isempty(answer5)
   answer5 = 'y';
end
disp('  ');
if strcmp(answer5,yes) == 1
   xx = trajenu(:,1);
   yy = trajenu(:,2);
   figure(2)
   plot(xx,yy),grid,...
   xlabel('ENU x-axis, in meters');
   ylabel('ENU y-axis, in meters');
   title('ENU x-y trajectory (origin is the reference point)');
   hold on
   plot(xx,yy,'*')
end

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

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

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