greatcir.m

惯性导航系统传递对准系统模型程序

function [lat_prof,lon_prof,tc_prof,totdist] = ...
   greatcir(latlonstart,latlonend,height,earthflg,dinc)

if nargin<5, distflg = 0; else distflg = 1; end
if nargin<4,error('insufficient number of input arguments'),end
if (earthflg ~= 0) & (earthflg ~= 1), error('EARTHFLG not specified correctly'),end

lat_start = latlonstart(1); lon_start = latlonstart(2);
lat_end = latlonend(1); lon_end = latlonend(2);
lat1 = lat_start; lon1 = lon_start;
lat2 = lat_end; lon2 = lon_end;
%
if earthflg == 0,
   Ravg_m = 6378137 + height;
else
   ae = 6378137;
   e = 0.0818191908426;
   x1 = ( ae/(sqrt(1-e*e*sin(lat1)*sin(lat1))) + height )*cos(lat2);
   y1 = ( ae*(1-e*e)/(sqrt(1-e*e*sin(lat1)*sin(lat1))) + height )*sin(lat2);
   x2 = ( ae/(sqrt(1-e*e*sin(lat2)*sin(lat2))) + height )*cos(lat2);
   y2 = ( ae*(1-e*e)/(sqrt(1-e*e*sin(lat2)*sin(lat2))) + height )*sin(lat2);
   R1 = norm([x1 y1]);
   R2 = norm([x2 y2]);
   Ravg_m = mean([R1 R2]);
end
Ravg_nmi = (Ravg_m/0.3048)/6076;
%
d_rad = acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2));
d_nm = d_rad*Ravg_nmi;
%
if distflg == 0,
   d_inc_nm = d_nm/100;
else
   d_inc_nm = dinc;
end
d_inc_rad = d_inc_nm/Ravg_nmi;

N = ceil(d_nm/d_inc_nm);
for i = 1:N,
   d_rad = acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2));
   tc = acos((sin(lat2)-sin(lat1)*cos(d_rad))/(sin(d_rad)*cos(lat1)));
   if sin(lon2-lon1) < 0,
      tc = 2*pi-tc;
   end
   if ~isreal(tc), tc = real(tc); end
   if tc > (2*pi - 1e-4), tc = 0; end
   tc_prof(i) = tc;

   lat(i) = asin(sin(lat1)*cos(d_inc_rad)+cos(lat1)*sin(d_inc_rad)*cos(tc));
   if abs(cos(lat(i))) < 3*eps,
      lon(i) = lon1;
   else,
      atmp = sin(tc)*sin(d_inc_rad)/cos(lat(i));
      if atmp < -1,
         lon(i) = mod(lon1+asin(-1)+pi,2*pi)-pi;
      elseif atmp > 1,
         lon(i) = mod(lon1+asin(1)+pi,2*pi)-pi;
      else,
         lon(i) = mod(lon1+asin(sin(tc)*sin(d_inc_rad)/cos(lat(i)))+pi,2*pi)-pi;
      end
   end
   lat1 = lat(i);
   lon1 = lon(i);
end
lat_prof = lat;
lon_prof = lon;
totdist = d_nm;