elems.m

matlab编写的轨道坐标系变换程序 地心直角坐标系与轨道参数互换

%  
%            elems.m 

%  Mfile for converting r, v vectors to orbital elements


%    Inputs : x vector DU,DU/TU unit
%             x(1-3): x,y,z; x(4-6): vx,vy,vz
%    Outputs: Classical elements (sma DU, all angles in deg),
%             stored in z vector (6x1)

function z = elems(x);

%  constants
r2d = 180/pi;
d2r = 1/r2d;

one = 1 - 1.e-12;
eps = 1e-12;

%  Input r and v

RX = x(1);
RY = x(2);
RZ = x(3);

VX = x(4);
VY = x(5);
VZ = x(6);

%   COMPUTE THE ANGULAR MOMENTUM VECTOR

HX = RY*VZ - RZ*VY;
HY = -( RX*VZ - RZ*VX );
HZ = RX*VY - RY*VX;
H = sqrt(HX^2 + HY^2 + HZ^2 );

%  NODE VECTOR

NX = -HY;
NY = HX;
NZ = 0;
N_norm = sqrt( NX^2 + NY^2 );

%   R AND V MAG'S

R = sqrt( RX^2 + RY^2 + RZ^2 );
V = sqrt( VX^2 + VY^2 + VZ^2 );
ARG = V^2 - 1/R;
RDOTV = RX*VX + RY*VY + RZ*VZ;

%  SMA

ENERGY = V^2/2 - 1.D0/R;
sma = -1.D0/(2.D0*ENERGY);

%  ECCENTRICITY

ECCX = ARG*RX - RDOTV*VX;
ECCY = ARG*RY - RDOTV*VY;     
ECCZ = ARG*RZ - RDOTV*VZ;
ecc = sqrt( ECCX^2 + ECCY^2 + ECCZ^2 );

%  INCLINATION

COSI = HZ/H;
incl = acos(COSI);

%  ASCENDING NODE (RAAN)

raan = 0;
argp = 0;
theta = 0;

if N_norm > eps
   
  COSRAN = NX/N_norm;
  
  if COSRAN > one 
     COSRAN = one;
  end
  
  if COSRAN < -one
     COSRAN = -one;
  end
  
  raan = acos(COSRAN);
  
  if NY < 0 
     raan = 2*pi - raan;
  end
  
end
   

%  ARGUMENT OF PERIGEE

if ecc < eps

%    Orbit is equatorial and circular

%    Set xnu = angle from x-axis to radius

   xdotr = RX;
   ctheta = RX/R;
   
   if ctheta > one 
      ctheta = one;
   end
   
   if ctheta < -one 
      ctheta = -one;
   end
   
   theta = acos(ctheta);
   
   if RY < 0 
      theta = 2*pi - theta;
   end
   
end

if N_norm < eps
   
   edotx = ECCX;
   exmag = ecc;
   cargp = edotx/exmag;
   
else

  EDOTN = NX*ECCX + NY*ECCY + NZ*ECCZ;
  ENMAG = N_norm*ecc;
  cargp = EDOTN/ENMAG;

end


if cargp > one 
   cargp = one;
end

if cargp < -one 
   cargp = -one;
end

argp = acos(cargp);

if ECCZ < 0
   argp = 2*pi - argp;
end


%   TRUE ANOMALY

if ecc < eps 

%  Set xnu to angle from norm to r

 rdotn = RX*NX + RY*NY + RZ*NZ;
 ctheta = rdotn/(R*N_norm);
 
 if ctheta > one
    ctheta = one;
 end
 
 if ctheta < -one
    ctheta = -one;
 end

 theta = acos(ctheta);
 
 if RZ < 0
    theta = 2*pi - theta;
 end
 
else
       

 EDOTR = ECCX*RX + ECCY*RY + ECCZ*RZ;
 ER = ecc*R;
 ctheta = EDOTR/ER;
 
 if ctheta > one
    ctheta = one;
 end
 
 if ctheta < -one
    ctheta = -one;
 end

 theta = acos(ctheta);
 
 
 if RDOTV < 0
    theta = 2*pi - theta;
 end
 

end

incl_deg = incl*r2d;
raan_deg = raan*r2d;
argp_deg = argp*r2d;
theta_deg = theta*r2d;

% others

p = sma*(1 - ecc^2 );
rp = p/(1 + ecc);
ra = p/(1 - ecc);

%  store for output

z(1) = sma;
z(2) = ecc;
z(3) = incl_deg;
z(4) = raan_deg;
z(5) = argp_deg;
z(6) = theta_deg;