xgcdr.m

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

%                                xgcdr.m
%  Scope:   This MATLAB program determines great circle dead reckoning
%           trajectory assuming a spherical earth with radius equal to 
%           the local radius and constant altitude; WGS-84 constants are
%           used.
%  Usage:   xgcdr
%  Inputs:  - selection of the default input data or enter the following 
%             input data from keyboard:
%             - initial latitude, in radians
%             - initial longitude, in radians
%             - altitude, in meters
%             - heading, in radians
%             - speed, in meters/second
%             - initial time, in seconds
%             - time step, in seconds
%             - number of steps
%           - selection of the output data file or exit the results on screen,
%           - selection of the plot option
%  Outputs: - the recorded output data (at each time step) are as follows:
%             - time, in seconds,
%             - latitude, in radians,
%             - longitude, in radians,
%             - altitude, in meters,
%             - velocity north component, in meters/second,
%             - velocity east component, in meters/second
%             on a specified file or on screen
%           - plot of the generated longitude-latitude trajectory (optional)
%  External Matlab macros used:  convcon, geodes, wgs84con
%  Last update:  11/07/00
%  Copyright (C) 1998-00 by LL Consulting. All Rights Reserved.

clear
close all
yes = 'y';

wgs84con
global a_smaxis eccentr2 flatness b_smaxis eprime2  % data used from wgs84con
convcon
global  deg_rad            % data used from convcon

%  Initialization at the start of great circle dead reckoning    

disp('  ');
answer1 = input('Do you want to use the default data? (y/n)[n] ','s');
disp('  ');
if (strcmp(answer1,yes) == 1)
   lat0 = 34.*deg_rad;           % in radians
   lon0 = -118.*deg_rad;         % in radians
   alt0 = 5000.0;                % in meters
   heading0 = 0.05;              % in radians
   speed0 = 200.0;               % in meters/second
   time_init = 0.0;              % in seconds
   time_incr = 300.;             % in seconds
   nr_steps = 10;
else
   lat0 = input('Specify the initial latitude, in radians --> ');
   lon0 = input('Specify the initial longitude, in radians --> ');
   alt0 = input('Specify the initial altitude, in meters --> ');
   heading0 = input('Specify the heading, in radians --> ');
   speed0 = input('Specify the speed, in meters/second --> ');
   time_init = input('Specify the initial time, in seconds --> ');
   time_incr = input('Specify the time step, in seconds --> ');
   nr_steps = input('Specify the number of steps --> ');
end

sinh0 = sin(heading0);
cosh0 = cos(heading0);
sinlat0 = sin(lat0);
coslat0 = cos(lat0);
sinlon0 = sin(lon0);
coslon0 = cos(lon0);
rn = a_smaxis / sqrt(1.0 - (eccentr2 * sinlat0 * sinlat0));

%  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
if strcmp(answer2,yes) == 1
   disp('  ');
   f2 = input('Specify the output filename  -->  ','s');
else
   f2 = 1;     %   output to the screen
end
fprintf(f2,'\n*****************************************************************************\n\n');
fprintf(f2,'*****Initial conditions:\n');
fprintf(f2,'Latitude     (radians) =  %14.8f\n',lat0);
fprintf(f2,'Longitude    (radians) =  %14.8f\n',lon0);
fprintf(f2,'Altitude      (meters) =  %14.8f\n',alt0);
fprintf(f2,'Heading      (radians) =  %14.8f\n',heading0);
fprintf(f2,'Speed  (meters/second) =  %14.8f\n',speed0);
fprintf(f2,'Time_init    (seconds) =  %14.8f\n',time_init);
fprintf(f2,'Time_step    (seconds) =  %14.8f\n',time_incr);
fprintf(f2,'Number of time steps   = %6.0f\n\n',nr_steps);
fprintf(f2,'*****Great Circle Dead Reckoning Trajectory \n\n ');
fprintf(f2,'   Time      Latitude   Longitude  Altitude   Vel_N    Vel_E    Distance\n');
fprintf(f2,'    (sec)       (rad)      (rad)       (m)    (m/sec)  (m/sec)      (m) \n');
vn0 = speed0 * cosh0;
ve0 = speed0 * sinh0;
dist0 = 0;
fprintf(f2,'%10.3f %11.8f %11.8f %9.3f %8.3f %8.3f  %10.3f\n', ...
       time_init, lat0, lon0, alt0, vn0, ve0, dist0);

%   Execute the great circle dead reckoning computation for each time step

lat(1) = lat0;
lon(1) = lon0;
time = time_init;
for i = 2:(nr_steps+1)
    time = time + time_incr;
    s = speed0*time/(rn + alt0);
    cos_s = cos(s);
    sin_s = sin(s);
    lat_new = asin(cos_s*sinlat0 + sin_s*cosh0*coslat0);
    cos_lat_new = cos(lat_new);
    if (cos_lat_new < 2.0e-7) 
        cos_lat_new = 2.0e-7; 
    end   
    tempf = (cos_s*coslat0*coslon0 - sin_s*(sinh0*sinlon0 + ...
		     cosh0*sinlat0*coslon0))/cos_lat_new;
    lon_new = acos(tempf);

%  Determine North and East velocities  

    vn = speed0*(-sin_s*sinlat0 + cos_s*cosh0*coslat0)/cos_lat_new;
    ve = sqrt(speed0*speed0 - vn*vn);

    y = cos_s*sinlon0*coslat0 + sin_s*sinh0*coslon0 - ...
  			 sin_s*cosh0*sinlon0*sinlat0;
    if (y < 0) 
        lon_new = -lon_new;
    end   

    alt_new = alt0;
    lat(i) = lat_new;
    lon(i) = lon_new;
    
%   Determine the segment length (range)

    temp_lat1 = lat(i-1)*rad_deg;
    temp_lat2 = lat(i)*rad_deg;
    temp_lon1 = lon(i-1)*rad_deg;
    temp_lon2 = lon(i)*rad_deg;
    [range,bearing1,bearing2] = geodes(temp_lat1,temp_lon1,temp_lat2,temp_lon2);
    dist(i) = range;

%   Save the data

    fprintf(f2,'%10.3f %11.8f %11.8f %9.3f %8.3f %8.3f  %10.3f\n', ...
                time, lat_new, lon_new, alt_new, vn, ve, dist(i));
    
end 
 
fprintf(f2,'\n*****************************************************************************\n');
disp('  ');

answer3 = input('Do you want to plot the trajectory? (y/n)[y] ','s');
if  isempty(answer3)
   answer3 = 'y';
end
disp('  ');
if strcmp(answer3,yes) == 1
   range_total = sum(dist);
   temp = ['Total range = ' num2str(range_total) ' meters' ];
   disp('Place the text with total range on the graph; ');
   tempp = input('      Press <Enter> or <Return> key to start ... ');
   figure(1)
   plot(lon,lat,'*'),grid,...
   xlabel('Longitude, in radians');
   ylabel('Latitude, in radians');
   title('Great circle dead reckoning trajectory');
   hold on
   plot(lon,lat)
   gtext(temp)           %  mouse placement of the text on a graph
   disp('   ');
   temp = input('Press <Enter> or <Return> key to continue ... ');
   disp('   ');
end

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