Description of mga

0001 % ------------------------------------------------------------------------
0002 % This source file is part of the 'ESA Advanced Concepts Team's
0003 % Space Mechanics Toolbox' software.
0004 %
0005 % The source files are for research use only,
0006 % and are distributed WITHOUT ANY WARRANTY. Use them on your own risk.
0007 %
0008 % Copyright (c) 2004-2007 European Space Agency
0009 % ------------------------------------------------------------------------
0010 %
0011 
0012 function [J,DVvec] = mga_dsm(t,problem)
0013 %
0014 %
0015 %Programmed by:   Claudio Bombardelli (ESA/ACT)
0016 %                 Dario Izzo          (ESA/ACT)
0017 %Date:                  15/03/2007
0018 %Revision:              4
0019 %Tested by:             C.Bombardelli
0020 %
0021 %  Computes the DeltaV cost function of a Multiple Gravity Assist trajectory
0022 %  with a Deep Space Maneuver between each planet pair
0023 %  N.B.: All swing-bys are UNPOWERED (thrust is only present at each dsm)
0024 %  It takes as input a sequence of planets P1,Pn and a decision vector t
0025 %
0026 %  The flyby/dsm sequence is:  P1/d1/P2/d2/../dn-1/Pn
0027 %
0028 
0029 % DECISION VECTOR DEFINITION:
0030 %
0031 %  t(1)=  epoch of departure MJD2000 from first planet (not necessarily earth)
0032 %  t(2)=  magnitude hyperbolic escape velocity from first planet
0033 %  t(3,4)= u,v variables for the hyperbolic velocity orientation wrt Earth
0034 %  velocity at departure
0035 %  t(5..n+3)= planet-to-planet Time of Flight [ToF] (days)
0036 %  t(n+4..2n-2)= fraction of ToF at which the DSM occurs
0037 %  t(2n+3..3n) = perigee fly-by radius for planets P2..Pn-1, non-dimensional wrt planetary radii
0038 %  t(3n+1..4n-2) = rotation gamma of the bplane-component of the swingby outgoing velocity (v_rel_out)
0039 %  [take n_r=cross(v_rel_in,v_planet_helio) if you rotate n_r by +gamma around v_rel_in
0040 %  you obtain the projection of v_rel_out on the b-plane]
0041 %  Vector (Vout) around the axis of the incoming swingby velocity vector (Vin)
0042 %
0043 
0044 %Usage:     [J,DVvec,dsm_epoch,flyby_epoch] = mga_dsm(t,MGADSMproblem)
0045 %Outputs:
0046 %           J:     Cost function = depends on the problem:
0047 %                  orbit insertion: total DV from propulsion system (V_launcher not counted)
0048 %                  gtoc1= (s/c final mass)*v_asteroid'*vrel_ast_sc
0049 %                  asteroid deflection-> 1/d with d=deflecion on the earth-asteroid lineofsight
0050 %           DVvec: vector of all DV maneuvers
0051 %           dsm_epoch: vector of epochs (mjd2000) corresponding to each DV maneuver
0052 %           flyby_epoch: vector of epochs (mjd2000) corresponding to each flyby
0053 %
0054 %
0055 %Inputs:
0056 %           t:         decision vector
0057 %               MGADSMproblem = struct array defining the problem, i.e.
0058 %               MGADSMproblem.sequence:  planet sequence. Example [3 3 4]= [E E M]. A
0059 %                      negative sign represents a retrograde orbit after the fly-by
0060 %               MGADSMproblem.objective.type: type of objective function (e.g.
0061 %                       'orbit insertion','rndv','gtoc1')
0062 %               MGADSMproblem.objective.rp: pericentre radius of the
0063 %                       target orbit if type = 'orbit insertion'
0064 %               MGADSMproblem.objective.e:  eccentricity of the target
0065 %                       orbit if type = 'orbit insertion'
0066 %               MGADSMproblem.bounds: decision vector upper and lower
0067 %               bounds for the optimiser
0068 %               MGADSMproblem.yplot:     1-> plot trajectory, 0-> don't
0069 %
0070 %
0071 %*********  IMPORTANT NOTE (SINGULARITY)   ************
0072 %
0073 % The routine is singular when the S/C relative incoming  velocity to a planet v_rel_in
0074 % is parallel to the heliocentric velocity of that planet.
0075 %
0076 % One can move the singularity elsewhere by changing the definition of the
0077 % angle gamma.
0078 % For example one possibility is to define gamma as follows:
0079 % [take n_r=cross(v_rel_in,r_planet_helio) and rotate n_r by +gamma around v_rel_in
0080 % in this case is singular for v_rel_in parallel to r_planet_helio
0081 %
0082 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0083 
0084 sequence= problem.sequence;          %THE PLANETs SEQUENCE
0085 yplot = problem.yplot;             %This option is deactivated in this version of the mga_dsm.m
0086 
0087 switch problem.objective.type
0088     case 'orbit insertion'
0089         rp_target= problem.objective.rp; % radius of pericentre at capture
0090         e_target=problem.objective.e;   % Eccentricity of the target orbit at capture
0091     case 'total DV orbit insertion'
0092         rp_target= problem.objective.rp; % radius of pericentre at capture
0093         e_target=problem.objective.e;   % Eccentricity of the target orbit at capture
0094     case 'gtoc1'
0095         Isp=problem.objective.Isp;
0096         mass = problem.objective.mass;
0097     case 'time to AUs'
0098         AUdist = problem.objective.AU;
0099         DVtotal = problem.objective.DVtot;
0100         DVonboard = problem.objective.DVonboard;
0101 end
0102 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0103 
0104 
0105 %**************************************************************************
0106 %Definition of the gravitational constants of the various planets
0107 %(used in the powered swing-by routine) and of the sun (used in the lambert
0108 %solver routine)
0109 %**************************************************************************
0110 
0111 mu(1)=22321;          %                          Mercury
0112 mu(2)=324860;         %Gravitational constant of Venus
0113 mu(3)=398601.19;      %                          Earth
0114 mu(4)=42828.3;        %                          Mars
0115 mu(5)=126.7e6;        %                          Jupiter
0116 mu(6)=37.9e6;         %                          Saturn
0117 
0118 muSUN=1.32712428e+11; %Gravitational constant of Sun
0119 
0120 
0121 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0122 %  Definition of planetari radii
0123 %
0124 RPL(1)=2440; % Mercury
0125 RPL(2)=6052; % Venus
0126 RPL(3)=6378; % Earth
0127 RPL(4)=3397; % Mars
0128 RPL(5)=71492;% Jupiter
0129 RPL(6)=60268;% Saturn
0130 
0131 
0132 %**************************************************************************
0133 % Decision vector definition
0134 
0135 tdep=t(1);         % departure epoch (MJD2000)
0136 VINF=t(2);         %Hyperbolic escape velocity (km/sec)
0137 udir=t(3);            %Hyperbolic escape velocity var1 (non dim)
0138 vdir=t(4);            %Hyperbolic escape velocity var2 (non dim)
0139 N=length(sequence);
0140 
0141 %Preallocating memory increase speed
0142 tof = zeros(N-1,1);
0143 alpha = zeros(N-1,1);
0144 
0145 for i=1:N-1
0146     tof(i)=t(i+4); %planet-to-planet Time of Flight [ToF] (days)
0147     alpha(i)=t(N+i+3); %fraction of ToF at which the DSM occurs
0148 end
0149 
0150 
0151 %If we are optimising to reach a given distance in the shortest time ('time
0152 %to AUs) then the decision vector needs to include also r_p and bincl at
0153 %the last planet, otherwise not
0154 
0155 if strcmp(problem.objective.type,'time to AUs')
0156     rp_non_dim=zeros(N-1,1);        %initialization gains speed
0157     gamma=zeros(N-1,1);
0158     for i=1:N-1
0159         rp_non_dim(i)=t(i+2*N+2); % non-dim perigee fly-by radius of planets P2..Pn (i=1 refers to the second planet)
0160         gamma(i)=t(3*N+i);        % rotation of the bplane-component of the swingby outgoing
0161         % velocity  Vector (Vout) around the axis of the incoming swingby velocity vector (Vin)
0162     end
0163 else
0164     rp_non_dim=zeros(N-2,1);        %initialization gains speed
0165     gamma=zeros(N-2,1);
0166     for i=1:N-2
0167         rp_non_dim(i)=t(i+2*N+2); % non-dim perigee fly-by radius of planets P2..Pn-1 (i=1 refers to the second planet)
0168         gamma(i)=t(3*N+i);        % rotation of the bplane-component of the swingby outgoing
0169         % velocity  Vector (Vout) around the axis of the incoming swingby velocity vector (Vin)
0170     end
0171 end
0172 
0173 %**************************************************************************
0174 %Evaluation of position and velocities of the planets
0175 %**************************************************************************
0176 
0177 N=length(sequence);
0178 
0179 r= zeros(3,N);
0180 v= zeros(3,N);
0181 
0182 muvec=zeros(N,1);
0183 Itime = zeros(N);
0184 dT=zeros(1,N);
0185 
0186 seq = abs (sequence);
0187 
0188 T=tdep;
0189 dT(1:N-1)=tof;
0190 
0191 for i=1:N
0192     Itime(i)=T;
0193     if seq(i)<10
0194         pleph_an( T , seq(i));
0195         [r(:,i),v(:,i)]=pleph_an( T , seq(i)); %positions and velocities of solar system planets
0196         muvec(i)=mu(seq(i)); %gravitational constants
0197     else
0198         [r(:,i),v(:,i)]=CUSTOMeph( mjd20002jed(T) , ...
0199             problem.customobject(seq(i)).epoch, ...
0200             problem.customobject(seq(i)).keplerian , 1); %positions and velocities of custom object
0201         muvec(i)=problem.customobject(seq(i)).mu; %gravitational constant of custom object
0202 
0203     end
0204 
0205     T=T+dT(i);
0206 
0207 end
0208 
0209 
0210 
0211 if strcmp(problem.objective.type,'time to AUs')
0212     rp=zeros(N-1,1);        %initialization gains speed
0213     for i=1:N-1
0214         rp(i)= rp_non_dim(i)*RPL(seq(i+1)); %dimensional flyby radii (i=1 corresponds to 2nd planet)
0215     end
0216 else
0217     rp=zeros(N-2,1);        %initialization gains speed
0218     for i=1:N-2
0219         rp(i)= rp_non_dim(i)*RPL(seq(i+1)); %dimensional flyby radii (i=1 corresponds to 2nd planet)
0220     end
0221 end
0222 
0223 
0224 %**************************************************************************
0225 %%%% FIRST BLOCK (P1 to P2)
0226 
0227 %Spacecraft position and velocity at departure
0228 
0229 vtemp= cross(r(:,1),v(:,1));
0230 
0231 iP1= v(:,1)/norm(v(:,1));
0232 zP1= vtemp/norm(vtemp);
0233 jP1= cross(zP1,iP1);
0234 
0235 
0236 theta=2*pi*udir;         %See Picking a Point on a Sphere
0237 phi=acos(2*vdir-1)-pi/2; %In this way: -pi/2<phi<pi/2 so phi can be used as out-of-plane rotation
0238 
0239 %vinf=VINF*(-sin(theta)*iP1+cos(theta)*cos(phi)*jP1+sin(phi)*cos(theta)*zP1);
0240 vinf=VINF*(cos(theta)*cos(phi)*iP1+sin(theta)*cos(phi)*jP1+sin(phi)*zP1);
0241 
0242 v_sc_pl_in(:,1)=v(:,1); %Spacecraft absolute incoming velocity at P1
0243 v_sc_pl_out(:,1)=v(:,1)+vinf; %Spacecraft absolute outgoing velocity at P1
0244 
0245 %Days from P1 to DSM1
0246 tDSM(1)=alpha(1)*tof(1);
0247 
0248 %Computing S/C position and absolute incoming velocity at DSM1
0249 [rd(:,1),v_sc_dsm_in(:,1)]=propagateKEP(r(:,1),v_sc_pl_out(:,1),tDSM(1)*24*60*60,muSUN);
0250 
0251 %Evaluating the Lambert arc from DSM1 to P2
0252 
0253 lw=vett(rd(:,1),r(:,2));
0254 lw=sign(lw(3));
0255 if lw==1
0256     lw=0;
0257 else
0258     lw=1;
0259 end
0260 [v_sc_dsm_out(:,1),v_sc_pl_in(:,2)]=lambertI(rd(:,1),r(:,2),tof(1)*(1-alpha(1))*24*60*60,muSUN,lw);
0261 
0262 %First Contribution to DV (the 1st deep space maneuver)
0263 DV=zeros(N-1,1);
0264 DV(1)=norm(v_sc_dsm_out(:,1)-v_sc_dsm_in(:,1));
0265 
0266 
0267 %****************************************
0268 % INTERMEDIATE BLOCK
0269 
0270 tDSM=zeros(N-1,1);
0271 for i=1:N-2
0272 
0273     %Evaluation of the state immediately after Pi
0274 
0275     v_rel_in=v_sc_pl_in(:,i+1)-v(:,i+1);
0276 
0277     e=1+rp(i)/muvec(i+1)*v_rel_in'*v_rel_in;
0278 
0279     beta_rot=2*asin(1/e);              %velocity rotation
0280 
0281     ix=v_rel_in/norm(v_rel_in);
0282     % ix=r_rel_in/norm(v_rel_in);  % activating this line and disactivating the one above
0283     % shifts the singularity for r_rel_in parallel to v_rel_in
0284 
0285     iy=vett(ix,v(:,i+1)/norm(v(:,i+1)))';
0286     iy=iy/norm(iy);
0287     iz=vett(ix,iy)';
0288     iVout = cos(beta_rot) * ix + cos(gamma(i))*sin(beta_rot) * iy + sin(gamma(i))*sin(beta_rot) * iz;
0289     v_rel_out=norm(v_rel_in)*iVout;
0290 
0291     v_sc_pl_out(:,i+1)=v(:,i+1)+v_rel_out;
0292 
0293 
0294     %Days from Pi to DSMi
0295     tDSM(i+1)=alpha(i+1)*tof(i+1);
0296 
0297 
0298     %Computing S/C position and absolute incoming velocity at DSMi
0299     [rd(:,i+1),v_sc_dsm_in(:,i+1)]=propagateKEP(r(:,i+1),v_sc_pl_out(:,i+1),tDSM(i+1)*24*60*60,muSUN);
0300 
0301 
0302     %Evaluating the Lambert arc from DSMi to Pi+1
0303 
0304     lw=vett(rd(:,i+1),r(:,i+2));
0305     lw=sign(lw(3));
0306     if lw==1
0307         lw=0;
0308     else
0309         lw=1;
0310     end
0311     [v_sc_dsm_out(:,i+1),v_sc_pl_in(:,i+2)]=lambertI(rd(:,i+1),r(:,i+2),tof(i+1)*(1-alpha(i+1))*24*60*60,muSUN,lw);
0312 
0313     %DV contribution
0314     DV(i+1)=norm(v_sc_dsm_out(:,i+1)-v_sc_dsm_in(:,i+1));
0315 
0316 end
0317 
0318 %************************************************************************
0319 % FINAL BLOCK
0320 %
0321 %1)Evaluation of the arrival DV
0322 %
0323 DVrel=norm(v(:,N)-v_sc_pl_in(:,N)); %Relative velocity at target planet
0324 
0325 
0326 switch problem.objective.type
0327     case 'orbit insertion'
0328         DVper=sqrt(DVrel^2+2*muvec(N)/rp_target);  %Hyperbola
0329         DVper2=sqrt(2*muvec(N)/rp_target-muvec(N)/rp_target*(1-e_target)); %Ellipse
0330         DVarr=abs(DVper-DVper2);
0331     case 'total DV orbit insertion'
0332         DVper=sqrt(DVrel^2+2*muvec(N)/rp_target);  %Hyperbola
0333         DVper2=sqrt(2*muvec(N)/rp_target-muvec(N)/rp_target*(1-e_target)); %Ellipse
0334         DVarr=abs(DVper-DVper2);
0335     case 'rndv'
0336         DVarr = DVrel;
0337     case 'total DV rndv'
0338         DVarr = DVrel;
0339     case 'gtoc1'
0340         DVarr = DVrel;
0341     case 'time to AUs'  %no DVarr is considered
0342         DVarr = 0;
0343 end
0344 
0345 DV(N)=DVarr;
0346 
0347 
0348 %
0349 %**************************************************************************
0350 %Evaluation of total DV spent by the propulsion system
0351 %**************************************************************************
0352 
0353 switch problem.objective.type
0354     case 'gtoc1'
0355         DVtot=sum(DV(1:N-1));
0356     otherwise
0357         DVtot=sum(DV);
0358 end
0359 
0360 
0361 DVvec=[VINF ; DV];
0362 
0363 
0364 
0365 
0366 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0367 %Finally our objective function is:
0368 switch problem.objective.type
0369     case 'total DV orbit insertion'
0370         J= DVtot+VINF;
0371     case 'total DV rndv'
0372         J= DVtot+VINF;
0373     case 'orbit insertion'
0374         J= DVtot;
0375     case 'rndv'
0376         J= DVtot;
0377     case 'gtoc1'
0378         mass_fin = mass * exp (- DVtot/ (Isp/1000 * 9.80665));
0379         J = 1/(mass_fin * abs((v_sc_pl_in(:,N)-v(:,N))'* v(:,N)));
0380     case 'time to AUs'
0381         %non dimensional units
0382         AU  = 149597870.66;
0383         V = sqrt(muSUN/AU);
0384         T = AU/V;
0385         %evaluate the state of the spacecraft after the last fly-by
0386         v_rel_in=v_sc_pl_in(:,N)-v(:,N);
0387         e=1+rp(N-1)/muvec(N)*v_rel_in'*v_rel_in;
0388         beta_rot=2*asin(1/e);              %velocity rotation
0389         ix=v_rel_in/norm(v_rel_in);
0390         % ix=r_rel_in/norm(v_rel_in);  % activating this line and disactivating the one above
0391         % shifts the singularity for r_rel_in parallel to v_rel_in
0392         iy=vett(ix,v(:,N)/norm(v(:,N)))';
0393         iy=iy/norm(iy);
0394         iz=vett(ix,iy)';
0395         iVout = cos(beta_rot) * ix + cos(gamma(N-1))*sin(beta_rot) * iy + sin(gamma(N-1))*sin(beta_rot) * iz;
0396         v_rel_out=norm(v_rel_in)*iVout;
0397         v_sc_pl_out(:,N)=v(:,N)+v_rel_out;
0398         t = time2distance(r(:,N)/AU,v_sc_pl_out(:,N)/V,AUdist);
0399         DVpen=0;
0400         if sum(DVvec)>DVtotal
0401             DVpen=DVpen+(sum(DVvec)-DVtotal);
0402         end
0403         if sum(DVvec(2:end))>DVonboard
0404             DVpen=DVpen+(sum(DVvec(2:end))-DVonboard);
0405         end
0406 
0407         J= (t*T/60/60/24 + sum(tof))/365.25 + DVpen*100;
0408         if isnan(J) 
0409             J = 100000;
0410         end
0411         if isnan(DV)
0412             J=100000;
0413         end
0414 end
0415 
0416 
0417 %--------------------------------------------------------------------------
0418 function [r,v] = propagateKEP(r0,v0,t,mu)
0419 %
0420 %Usage: [r,v] = propagateKEP(r0,v0,t)
0421 %
0422 %Inputs:
0423 %           r0:    column vector for the non dimensional position
0424 %           v0:    column vector for the non dimensional velocity
0425 %           t:     non dimensional time
0426 %
0427 %Outputs:
0428 %           r:    column vector for the non dimensional position
0429 %           v:    column vector for the non dimensional velocity
0430 %
0431 %Comments:  The function works in non dimensional units, it takes an
0432 %initial condition and it propagates it as in a kepler motion analytically.
0433 %
0434 %The matrix DD will be almost always the unit matrix, except for orbits
0435 %with little inclination in which cases a rotation is performed so that
0436 %par2IC is always defined
0437 DD=eye(3);
0438 h=vett(r0,v0);
0439 ih=h/norm(h);
0440 if abs(abs(ih(3))-1)<1e-3         %the abs is needed in cases in which the orbit is retrograde,
0441                                   %that would held ih=[0,0,-1]!!
0442     DD=[1,0,0;0,0,1;0,-1,0];      %Random rotation matrix that make the Euler angles well defined for the case
0443     r0=DD*r0;                     %For orbits with little inclination another ref. frame is used.
0444     v0=DD*v0;
0445 end
0446 
0447 E=IC2par(r0,v0,mu);  
0448 
0449 M0=E2M(E(6),E(2));
0450 if E(2)<1
0451     M=M0+sqrt(mu/E(1)^3)*t;
0452 else
0453     M=M0+sqrt(-mu/E(1)^3)*t;
0454 end
0455 E(6)=M2E(M,E(2));
0456 [r,v]=par2IC(E,mu);
0457 
0458 r=DD'*r;                    
0459 v=DD'*v;
0460 
0461 
0462 %--------------------------------------------------------------------------%
0463 function E=IC2par(r0,v0,mu)
0464 %
0465 %Usage: E = IC2par(r0,v0,mu)
0466 %
0467 %Inputs:
0468 %           r0:    column vector for the position
0469 %           v0:    column vector for the velocity
0470 %
0471 %Outputs:
0472 %           E:     Column Vectors containing the six keplerian parameters,
0473 %                  (a (negative for hyperbolas),e,i,OM,om,Eccentric Anomaly
0474 %                  (or Gudermannian whenever e>1))
0475 %
0476 %Comments:  The parameters returned are, of course, referred to the same
0477 %ref. frame in which r0,v0 are given. Units have to be consistent, and
0478 %output angles are in radians
0479 %The algorithm used is quite common and can be found as an example in Bate,
0480 %Mueller and White. It goes singular for zero inclination and for ni=pi
0481 %Note also that the anomaly in output ranges from -pi to pi
0482 %Note that a is negative for hyperbolae
0483 
0484 k=[0,0,1]';
0485 h=vett(r0,v0)';
0486 p=h'*h/mu;
0487 n=vett(k,h)';
0488 n=n/norm(n);
0489 R0=norm(r0);
0490 evett=vett(v0,h)'/mu-r0/R0;
0491 e=evett'*evett;
0492 E(1)=p/(1-e);
0493 E(2)=sqrt(e);
0494 e=E(2);
0495 E(3)=acos(h(3)/norm(h));
0496 E(5)=(acos((n'*evett)/e));
0497 if evett(3)<0
0498     E(5)=2*pi-E(5);
0499 end
0500 E(4)=acos(n(1));
0501 if n(2)<0
0502     E(4)=2*pi-E(4);
0503 end
0504 ni=real(acos((evett'*r0)/e/R0)); %real is to avoid problems when ni~=pi
0505 if (r0'*v0)<0
0506     ni=2*pi-ni;
0507 end
0508 EccAn=ni2E(ni,e);
0509 E(6)=EccAn;
0510 
0511 
0512 %--------------------------------------------------------------------------
0513 function E=ni2E(ni,e)
0514 if e<1
0515     E=2*atan(sqrt((1-e)/(1+e))*tan(ni/2)); %algebraic kepler's equation
0516 else
0517     E=2*atan(sqrt((e-1)/(e+1))*tan(ni/2)); %algebraic equivalent of kepler's equation in terms of the Gudermannian
0518 end
0519 
0520 
0521 
0522 %--------------------------------------------------------------------------
0523 function [r0,v0]=par2IC(E,mu)
0524 %
0525 %Usage: [r0,v0] = IC2par(E,mu)
0526 %
0527 %Outputs:
0528 %           r0:    column vector for the position
0529 %           v0:    column vector for the velocity
0530 %
0531 %Inputs:
0532 %           E:     Column Vectors containing the six keplerian parameters,
0533 %                  (a (negative fr hyperbolas),e,i,OM,om,Eccentric Anomaly
0534 %                    or Gudermannian if e>1)
0535 %           mu:    gravitational constant
0536 %
0537 %Comments:  The parameters returned are, of course, referred to the same
0538 %ref. frame in which r0,v0 are given. a can be given either in kms or AUs,
0539 %but has to be consistent with mu.All the angles must be given in radians.
0540 
0541 a=E(1);
0542 e=E(2);
0543 i=E(3);
0544 omg=E(4);
0545 omp=E(5);
0546 EA=E(6);
0547 
0548 
0549 % Grandezze definite nel piano dell'orbita
0550 if e<1
0551     b=a*sqrt(1-e^2);
0552     n=sqrt(mu/a^3);
0553 
0554     xper=a*(cos(EA)-e);
0555     yper=b*sin(EA);
0556 
0557     xdotper=-(a*n*sin(EA))/(1-e*cos(EA));
0558     ydotper=(b*n*cos(EA))/(1-e*cos(EA));
0559 else
0560     b=-a*sqrt(e^2-1);
0561     n=sqrt(-mu/a^3);   
0562     dNdzeta=e*(1+tan(EA)^2)-(1/2+1/2*tan(1/2*EA+1/4*pi)^2)/tan(1/2*EA+1/4*pi);
0563     
0564     xper = a/cos(EA)-a*e;
0565     yper = b*tan(EA);
0566     
0567     xdotper = a*tan(EA)/cos(EA)*n/dNdzeta;
0568     ydotper = b/cos(EA)^2*n/dNdzeta;
0569 end
0570 
0571 % Matrice di trasformazione da perifocale a ECI
0572 
0573 R(1,1)=cos(omg)*cos(omp)-sin(omg)*sin(omp)*cos(i);
0574 R(1,2)=-cos(omg)*sin(omp)-sin(omg)*cos(omp)*cos(i);
0575 R(1,3)=sin(omg)*sin(i);
0576 R(2,1)=sin(omg)*cos(omp)+cos(omg)*sin(omp)*cos(i);
0577 R(2,2)=-sin(omg)*sin(omp)+cos(omg)*cos(omp)*cos(i);
0578 R(2,3)=-cos(omg)*sin(i);
0579 R(3,1)=sin(omp)*sin(i);
0580 R(3,2)=cos(omp)*sin(i);
0581 R(3,3)=cos(i);
0582 
0583 % Posizione nel sistema inerziale
0584 
0585 r0=R*[xper;yper;0];
0586 v0=R*[xdotper;ydotper;0];
0587 
0588 
0589 
0590 %--------------------------------------------------------------------------
0591 function [v1,v2,a,p,theta,iter]=lambertI(r1,r2,t,mu,lw,N,branch)
0592 %
0593 %
0594 %This routine implements a new algorithm that solves Lambert's problem. The
0595 %algorithm has two major characteristics that makes it favorable to other
0596 %existing ones.
0597 %
0598 %   1) It describes the generic orbit solution of the boundary condition
0599 %   problem through the variable X=log(1+cos(alpha/2)). By doing so the
0600 %   graphs of the time of flight become defined in the entire real axis and
0601 %   resembles a straight line. Convergence is granted within few iterations
0602 %   for all the possible geometries (except, of course, when the transfer
0603 %   angle is zero). When multiple revolutions are considered the variable is
0604 %   X=tan(cos(alpha/2)*pi/2).
0605 %
0606 %   2) Once the orbit has been determined in the plane, this routine
0607 %   evaluates the velocity vectors at the two points in a way that is not
0608 %   singular for the transfer angle approaching to pi (Lagrange coefficient
0609 %   based methods are numerically not well suited for this purpose).
0610 %
0611 %   As a result Lambert's problem is solved (with multiple revolutions
0612 %   being accounted for) with the same computational effort for all
0613 %   possible geometries. The case of near 180 transfers is also solved
0614 %   efficiently.
0615 %
0616 %   We note here that even when the transfer angle is exactly equal to pi
0617 %   the algorithm does solve the problem in the plane (it finds X), but it
0618 %   is not able to evaluate the plane in which the orbit lies. A solution
0619 %   to this would be to provide the direction of the plane containing the
0620 %   transfer orbit from outside. This has not been implemented in this
0621 %   routine since such a direction would depend on which application the
0622 %   transfer is going to be used in.
0623 %
0624 %Usage: [v1,v2,a,p,theta,iter]=lambertI(r1,r2,t,mu,lw,N,branch)
0625 %
0626 %Inputs:
0627 %           r1=Position vector at departure (column)
0628 %           r2=Position vector at arrival (column, same units as r1)
0629 %           t=Transfer time (scalar)
0630 %           mu=gravitational parameter (scalar, units have to be
0631 %           consistent with r1,t units)
0632 %           lw=1 if long way is chosen
0633 %           branch='l' if the left branch is selected in a problem where N
0634 %           is not 0 (multirevolution)
0635 %           N=number of revolutions
0636 %
0637 %Outputs:
0638 %           v1=Velocity at departure        (consistent units)
0639 %           v2=Velocity at arrival
0640 %           a=semi major axis of the solution
0641 %           p=semi latus rectum of the solution
0642 %           theta=transfer angle in rad
0643 %           iter=number of iteration made by the newton solver (usually 6)
0644 %
0645 %
0646 %Preliminary control on the function call
0647 if nargin==5
0648     N=0;
0649 end
0650 if t<=0
0651     warning('Negative time as input')
0652     v1=[NaN;NaN;NaN];
0653     v2=[NaN;NaN;NaN];
0654     return
0655 end
0656 
0657 
0658 tol=1e-11;  %Increasing the tolerance does not bring any advantage as the
0659 %precision is usually greater anyway (due to the rectification of the tof
0660 %graph) except near particular cases such as parabolas in which cases a
0661 %lower precision allow for usual convergence.
0662 
0663 
0664 %Non dimensional units
0665 R=norm(r1);
0666 V=sqrt(mu/R);
0667 T=R/V;
0668 
0669 %working with non-dimensional radii and time-of-flight
0670 r1=r1/R;
0671 r2=r2/R;
0672 t=t/T;                     
0673 
0674 %Evaluation of the relevant geometry parameters in non dimensional units
0675 r2mod=norm(r2);
0676 theta=acos(r1'*r2/r2mod);
0677 
0678 if lw
0679     theta=2*pi-theta;
0680 end
0681 c=sqrt(1+r2mod^2-2*r2mod*cos(theta)); %non dimensional chord
0682 s=(1+r2mod+c)/2;                      %non dimensional semi-perimeter
0683 am=s/2;                               %minimum energy ellipse semi major axis
0684 lambda=sqrt(r2mod)*cos(theta/2)/s;    %lambda parameter defined in BATTIN's book
0685 
0686 
0687 
0688 %We start finding the log(x+1) value of the solution conic:
0689 %%NO MULTI REV --> (1 SOL)
0690 if N==0
0691     inn1=-.5233;    %first guess point
0692     inn2=.5233;     %second guess point
0693     x1=log(1+inn1);
0694     x2=log(1+inn2);
0695     y1=log(x2tof(inn1,s,c,lw,N))-log(t);
0696     y2=log(x2tof(inn2,s,c,lw,N))-log(t);
0697     
0698     %Newton iterations
0699     err=1;
0700     i=0;
0701     while ((err>tol) && (y1~=y2))
0702         i=i+1;
0703         xnew=(x1*y2-y1*x2)/(y2-y1);
0704         ynew=log(x2tof(exp(xnew)-1,s,c,lw,N))-log(t);
0705         x1=x2;
0706         y1=y2;
0707         x2=xnew;
0708         y2=ynew;
0709         err=abs(x1-xnew);
0710     end
0711     iter=i;
0712     x=exp(xnew)-1;
0713     
0714     
0715     %%MULTI REV --> (2 SOL) SEPARATING RIGHT AND LEFT BRANCH
0716 else 
0717     if branch=='l'
0718         inn1=-.5234;
0719         inn2=-.2234;
0720     else
0721         inn1=.7234;
0722         inn2=.5234;
0723     end
0724     x1=tan(inn1*pi/2);
0725     x2=tan(inn2*pi/2);
0726     y1=x2tof(inn1,s,c,lw,N)-t;
0727     
0728     y2=x2tof(inn2,s,c,lw,N)-t;
0729     err=1;
0730     i=0;
0731     
0732     %Newton Iteration
0733     while ((err>tol) && (i<60) && (y1~=y2))
0734         i=i+1;
0735         xnew=(x1*y2-y1*x2)/(y2-y1);
0736         ynew=x2tof(atan(xnew)*2/pi,s,c,lw,N)-t;
0737         x1=x2;
0738         y1=y2;
0739         x2=xnew;
0740         y2=ynew;
0741         err=abs(x1-xnew);        
0742     end
0743     x=atan(xnew)*2/pi;
0744     iter=i;
0745 end
0746 
0747 %The solution has been evaluated in terms of log(x+1) or tan(x*pi/2), we
0748 %now need the conic. As for transfer angles near to pi the lagrange
0749 %coefficient technique goes singular (dg approaches a zero/zero that is
0750 %numerically bad) we here use a different technique for those cases. When
0751 %the transfer angle is exactly equal to pi, then the ih unit vector is not
0752 %determined. The remaining equations, though, are still valid.
0753 
0754 
0755 a=am/(1-x^2);                       %solution semimajor axis
0756 %calcolo psi
0757 if x<1 %ellisse
0758     beta=2*asin(sqrt((s-c)/2/a));
0759     if lw
0760         beta=-beta;
0761     end
0762     alfa=2*acos(x);
0763     psi=(alfa-beta)/2;
0764     eta2=2*a*sin(psi)^2/s;
0765     eta=sqrt(eta2);
0766 else %iperbole
0767     beta=2*asinh(sqrt((c-s)/2/a));
0768     if lw
0769         beta=-beta;
0770     end
0771     alfa=2*acosh(x);
0772     psi=(alfa-beta)/2;
0773     eta2=-2*a*sinh(psi)^2/s;
0774     eta=sqrt(eta2);
0775 end
0776 p=r2mod/am/eta2*sin(theta/2)^2;     %parameter of the solution
0777 sigma1=1/eta/sqrt(am)*(2*lambda*am-(lambda+x*eta));
0778 ih=vers(vett(r1,r2)');
0779 if lw
0780     ih=-ih;
0781 end
0782 
0783 vr1 = sigma1;
0784 vt1 = sqrt(p);
0785 v1  = vr1 * r1   +   vt1 * vett(ih,r1)';
0786 
0787 vt2=vt1/r2mod;
0788 vr2=-vr1+(vt1-vt2)/tan(theta/2);
0789 v2=vr2*r2/r2mod+vt2*vett(ih,r2/r2mod)';
0790 v1=v1*V;
0791 v2=v2*V;
0792 a=a*R;
0793 p=p*R;
0794 
0795 
0796 
0797 %--------------------------------------------------------------------------
0798 %Subfunction that evaluates the time of flight as a function of x
0799 function t=x2tof(x,s,c,lw,N)  
0800 
0801 am=s/2;
0802 a=am/(1-x^2);
0803 if x<1 %ELLISSE
0804     beta=2*asin(sqrt((s-c)/2/a));
0805     if lw
0806         beta=-beta;
0807     end
0808     alfa=2*acos(x);
0809 else   %IPERBOLE
0810     alfa=2*acosh(x);
0811     beta=2*asinh(sqrt((s-c)/(-2*a)));
0812     if lw
0813         beta=-beta;
0814     end
0815 end
0816 t=tofabn(a,alfa,beta,N);
0817 
0818 
0819 %--------------------------------------------------------------------------
0820 function t=tofabn(sigma,alfa,beta,N)
0821 %
0822 %subfunction that evaluates the time of flight via Lagrange expression
0823 %
0824 if sigma>0
0825     t=sigma*sqrt(sigma)*((alfa-sin(alfa))-(beta-sin(beta))+N*2*pi);
0826 else
0827     t=-sigma*sqrt(-sigma)*((sinh(alfa)-alfa)-(sinh(beta)-beta));
0828 end
0829 
0830 %--------------------------------------------------------------------------
0831 function v=vers(V) 
0832 %subfunction that evaluates unit vectors
0833 v=V/sqrt(V'*V);
0834 
0835 
0836 
0837 %--------------------------------------------------------------------------
0838 function ansd = vett(r1,r2)  
0839 %
0840 %subfunction that evaluates vector product
0841 ansd(1)=(r1(2)*r2(3)-r1(3)*r2(2));
0842 ansd(2)=(r1(3)*r2(1)-r1(1)*r2(3));
0843 ansd(3)=(r1(1)*r2(2)-r1(2)*r2(1));
0844 
0845 
0846 
0847 
0848 %--------------------------------------------------------------------------
0849 function  [r,v,E]=pleph_an ( mjd2000, planet);
0850 %
0851 %The original version of this file was written in Fortran 77
0852 %and is part of ESOC library. The file has later been translated under the
0853 %name uplanet.m in Matlab, but that version was affected by several
0854 %mistakes. In particular Pluto orbit was affected by great uncertainties.
0855 %As a consequence its analytical eph have been substituted by a fifth order
0856 %polynomial least square fit generated by Dario Izzo (ESA ACT). JPL405
0857 %ephemerides (Charon-Pluto barycenter) have been used to produce the
0858 %coefficients. WARNING: Pluto ephemerides should not be used outside the
0859 %range 2000-2100;
0860 %
0861 %The analytical ephemerides of the nine planets of our solar system are
0862 %returned in rectangular coordinates referred to the ecliptic J2000
0863 %reference frame. Using analytical ephemerides instead of real ones (for
0864 %examples JPL ephemerides) is faster but not as accurate, especially for
0865 %the outer planets
0866 %
0867 %Date:                  28/05/2004
0868 %Revision:              1
0869 %Tested by:             ----------
0870 %
0871 %
0872 %Usage: [r,v,E]=pleph_an ( mjd2000 , IBODY );
0873 %
0874 %Inputs:
0875 %           mjd2000:    Days elapsed from 1st of January 2000 counted from
0876 %           midnight.
0877 %           planet:     Integer form 1 to 9 containing the number
0878 %           of the planet (Mercury, Venus, Earth, Mars, Jupiter, Saturn,
0879 %           Uranus, Neptune, Pluto)
0880 %
0881 %Outputs:
0882 %           r:         Column vector containing (in km) the position of the
0883 %                      planet in the ecliptic J2000
0884 %           v:         Column vector containing (in km/sec.) the velocity of
0885 %                      the planet in the ecliptic J2000
0886 %           E:         Column Vectors containing the six keplerian parameters,
0887 %                      (a,e,i,OM,om,Eccentric Anomaly)
0888 %
0889 %Xref:      the routine needs a variable mu on the workspace whose 10th
0890 %           element should be the sun gravitational parameter in KM^3/SEC^2
0891 
0892 global mu
0893 global AU
0894 
0895 
0896 
0897 RAD=pi/180;
0898 KM  = 149597870.66;        %Astronomical Unit
0899 %KM=AU; % Roberto, global variable AU was not initialized
0900 
0901 
0902 %
0903 %  T = JULIAN CENTURIES SINCE 1900
0904 %
0905 T   = (mjd2000 + 36525.00)/36525.00;
0906 TT  = T*T;
0907 TTT = T*TT;
0908 %
0909 %  CLASSICAL PLANETARY ELEMENTS ESTIMATION IN MEAN ECLIPTIC OF DATE
0910 %
0911 switch planet
0912     %
0913     %  MERCURY
0914     %
0915     case 1 
0916         E(1) = 0.38709860;
0917         E(2) = 0.205614210 + 0.000020460*T - 0.000000030*TT;
0918         E(3) = 7.002880555555555560 + 1.86083333333333333e-3*T - 1.83333333333333333e-5*TT;
0919         E(4) = 4.71459444444444444e+1 + 1.185208333333333330*T + 1.73888888888888889e-4*TT;
0920         E(5) = 2.87537527777777778e+1 + 3.70280555555555556e-1*T +1.20833333333333333e-4*TT;
0921         XM   = 1.49472515288888889e+5 + 6.38888888888888889e-6*T;
0922         E(6) = 1.02279380555555556e2 + XM*T;
0923         %
0924         %  VENUS
0925         %
0926     case 2
0927         E(1) = 0.72333160;
0928         E(2) = 0.006820690 - 0.000047740*T + 0.0000000910*TT;
0929         E(3) = 3.393630555555555560 + 1.00583333333333333e-3*T - 9.72222222222222222e-7*TT;
0930         E(4) = 7.57796472222222222e+1 + 8.9985e-1*T + 4.1e-4*TT;
0931         E(5) = 5.43841861111111111e+1 + 5.08186111111111111e-1*T -1.38638888888888889e-3*TT;
0932         XM   = 5.8517803875e+4 + 1.28605555555555556e-3*T;
0933         E(6) = 2.12603219444444444e2 + XM*T;
0934         %
0935         %  EARTH
0936         %
0937     case 3
0938         E(1) = 1.000000230;
0939         E(2) = 0.016751040 - 0.000041800*T - 0.0000001260*TT;
0940         E(3) = 0.00;
0941         E(4) = 0.00;
0942         E(5) = 1.01220833333333333e+2 + 1.7191750*T + 4.52777777777777778e-4*TT + 3.33333333333333333e-6*TTT;
0943         XM   = 3.599904975e+4 - 1.50277777777777778e-4*T - 3.33333333333333333e-6*TT;
0944         E(6) = 3.58475844444444444e2 + XM*T;
0945         %
0946         %  MARS
0947         %
0948     case 4
0949         E(1) = 1.5236883990;
0950         E(2) = 0.093312900 + 0.0000920640*T - 0.0000000770*TT;
0951         E(3) = 1.850333333333333330 - 6.75e-4*T + 1.26111111111111111e-5*TT;
0952         E(4) = 4.87864416666666667e+1 + 7.70991666666666667e-1*T - 1.38888888888888889e-6*TT - 5.33333333333333333e-6*TTT;
0953         E(5) = 2.85431761111111111e+2 + 1.069766666666666670*T +  1.3125e-4*TT + 4.13888888888888889e-6*TTT;
0954         XM   = 1.91398585e+4 + 1.80805555555555556e-4*T + 1.19444444444444444e-6*TT;
0955         E(6) = 3.19529425e2 + XM*T;
0956         
0957         %
0958         %  JUPITER
0959         %
0960     case 5
0961         E(1) = 5.2025610;
0962         E(2) = 0.048334750 + 0.000164180*T  - 0.00000046760*TT -0.00000000170*TTT;
0963         E(3) = 1.308736111111111110 - 5.69611111111111111e-3*T +  3.88888888888888889e-6*TT;
0964         E(4) = 9.94433861111111111e+1 + 1.010530*T + 3.52222222222222222e-4*TT - 8.51111111111111111e-6*TTT;
0965         E(5) = 2.73277541666666667e+2 + 5.99431666666666667e-1*T + 7.0405e-4*TT + 5.07777777777777778e-6*TTT;
0966         XM   = 3.03469202388888889e+3 - 7.21588888888888889e-4*T + 1.78444444444444444e-6*TT;
0967         E(6) = 2.25328327777777778e2 + XM*T;
0968         %
0969         %  SATURN
0970         %
0971     case 6
0972         E(1) = 9.5547470;
0973         E(2) = 0.055892320 - 0.00034550*T - 0.0000007280*TT + 0.000000000740*TTT;
0974         E(3) = 2.492519444444444440 - 3.91888888888888889e-3*T - 1.54888888888888889e-5*TT + 4.44444444444444444e-8*TTT;
0975         E(4) = 1.12790388888888889e+2 + 8.73195138888888889e-1*T -1.52180555555555556e-4*TT - 5.30555555555555556e-6*TTT;
0976         E(5) = 3.38307772222222222e+2 + 1.085220694444444440*T + 9.78541666666666667e-4*TT + 9.91666666666666667e-6*TTT;
0977         XM   = 1.22155146777777778e+3 - 5.01819444444444444e-4*T - 5.19444444444444444e-6*TT;
0978         E(6) = 1.75466216666666667e2 + XM*T;
0979         %
0980         %  URANUS
0981         %
0982     case 7
0983         E(1) = 19.218140;
0984         E(2) = 0.04634440 - 0.000026580*T + 0.0000000770*TT;
0985         E(3) = 7.72463888888888889e-1 + 6.25277777777777778e-4*T + 3.95e-5*TT;
0986         E(4) = 7.34770972222222222e+1 + 4.98667777777777778e-1*T + 1.31166666666666667e-3*TT;
0987         E(5) = 9.80715527777777778e+1 + 9.85765e-1*T - 1.07447222222222222e-3*TT - 6.05555555555555556e-7*TTT;
0988         XM   = 4.28379113055555556e+2 + 7.88444444444444444e-5*T + 1.11111111111111111e-9*TT;
0989         E(6) = 7.26488194444444444e1 + XM*T;
0990         %
0991         %  NEPTUNE
0992         %
0993     case 8
0994         E(1) = 30.109570;
0995         E(2) = 0.008997040 + 0.0000063300*T - 0.0000000020*TT;
0996         E(3) = 1.779241666666666670 - 9.54361111111111111e-3*T - 9.11111111111111111e-6*TT;
0997         E(4) = 1.30681358333333333e+2 + 1.0989350*T + 2.49866666666666667e-4*TT - 4.71777777777777778e-6*TTT;
0998         E(5) = 2.76045966666666667e+2 + 3.25639444444444444e-1*T + 1.4095e-4*TT + 4.11333333333333333e-6*TTT;
0999         XM   = 2.18461339722222222e+2 - 7.03333333333333333e-5*T;
1000         E(6) = 3.77306694444444444e1 + XM*T;
1001         %
1002         %  PLUTO
1003         %
1004     case 9
1005         %Fifth order polynomial least square fit generated by Dario Izzo
1006         %(ESA ACT). JPL405 ephemerides (Charon-Pluto barycenter) have been used to produce the coefficients.
1007         %This approximation should not be used outside the range 2000-2100;
1008         T=mjd2000/36525;
1009         TT=T*T;
1010         TTT=TT*T;
1011         TTTT=TTT*T;
1012         TTTTT=TTTT*T;
1013         E(1)=39.34041961252520 + 4.33305138120726*T - 22.93749932403733*TT + 48.76336720791873*TTT - 45.52494862462379*TTTT + 15.55134951783384*TTTTT;
1014         E(2)=0.24617365396517 + 0.09198001742190*T - 0.57262288991447*TT + 1.39163022881098*TTT - 1.46948451587683*TTTT + 0.56164158721620*TTTTT;
1015         E(3)=17.16690003784702 - 0.49770248790479*T + 2.73751901890829*TT - 6.26973695197547*TTT + 6.36276927397430*TTTT - 2.37006911673031*TTTTT;
1016         E(4)=110.222019291707 + 1.551579150048*T - 9.701771291171*TT + 25.730756810615*TTT - 30.140401383522*TTTT + 12.796598193159 * TTTTT;
1017         E(5)=113.368933916592 + 9.436835192183*T - 35.762300003726*TT + 48.966118351549*TTT - 19.384576636609*TTTT - 3.362714022614 * TTTTT;
1018         E(6)=15.17008631634665 + 137.023166578486*T + 28.362805871736*TT - 29.677368415909*TTT - 3.585159909117*TTTT + 13.406844652829 * TTTTT;
1019 end
1020 %
1021 %  CONVERSION OF AU INTO KM, DEG INTO RAD
1022 %
1023 E(1)     =     E(1)*KM;
1024 for  I = 3: 6
1025     E(I)     = E(I)*RAD;
1026 end
1027 E(6)     = mod(E(6), 2*pi);
1028 
1029 %Conversion from mean anomaly to eccentric anomaly via Kepler's equation
1030 EccAnom=M2E(E(6),E(2));
1031 E(6)=EccAnom;
1032 
1033 %Calcolo velocit�e posizione nel sistema J2000
1034 [r,v]=conversion(E);
1035 
1036 
1037 
1038 %--------------------------------------------------------------------------
1039 function [r,v] = conversion (E)
1040 %
1041 % Parametri orbitali
1042 
1043 muSUN=  1.327124280000000e+011;       %gravitational parameter for the sun
1044 
1045 a=E(1);
1046 e=E(2);
1047 i=E(3);
1048 omg=E(4);
1049 omp=E(5);
1050 EA=E(6);
1051 
1052 
1053 % Grandezze definite nel piano dell'orbita
1054 
1055 b=a*sqrt(1-e^2);
1056 n=sqrt(muSUN/a^3);
1057 
1058 xper=a*(cos(EA)-e);
1059 yper=b*sin(EA);
1060 
1061 xdotper=-(a*n*sin(EA))/(1-e*cos(EA));
1062 ydotper=(b*n*cos(EA))/(1-e*cos(EA));
1063 
1064 % Matrice di trasformazione da perifocale a ECI
1065 
1066 R(1,1)=cos(omg)*cos(omp)-sin(omg)*sin(omp)*cos(i);
1067 R(1,2)=-cos(omg)*sin(omp)-sin(omg)*cos(omp)*cos(i);
1068 R(1,3)=sin(omg)*sin(i);
1069 R(2,1)=sin(omg)*cos(omp)+cos(omg)*sin(omp)*cos(i);
1070 R(2,2)=-sin(omg)*sin(omp)+cos(omg)*cos(omp)*cos(i);
1071 R(2,3)=-cos(omg)*sin(i);
1072 R(3,1)=sin(omp)*sin(i);
1073 R(3,2)=cos(omp)*sin(i);
1074 R(3,3)=cos(i);
1075 
1076 % Posizione nel sistema inerziale
1077 
1078 r=R*[xper;yper;0];
1079 v=R*[xdotper;ydotper;0];
1080 
1081 
1082 
1083 %--------------------------------------------------------------------------
1084 function E=M2E(M,e)
1085 %
1086 i=0;
1087 tol=1e-10;
1088 err=1;
1089 E=M+e*cos(M);   %initial guess
1090 while err>tol && i<100
1091     i=i+1;
1092     Enew=E-(E-e*sin(E)-M)/(1-e*cos(E));
1093     err=abs(E-Enew);
1094     E=Enew;
1095 end
1096 
1097 
1098 %--------------------------------------------------------------------------
1099 function M=E2M(E,e)
1100 %
1101 %Transforms the eccentric anomaly to mean anomaly. All i/o in radians
1102 %
1103 %Usage: M=E2M(E,e)
1104 %
1105 %Inputs :   E : Eccentric anomaly or Gudermannian if e>1
1106 %           e : Eccentricity of considered orbit
1107 %
1108 %Output :   M : Mean anomaly or N if e>1
1109 
1110 if e<1 %Ellipse, E is the eccentric anomaly
1111     M=E-e*sin(E);
1112 else  %Hyperbola, E is the Gudermannian
1113     M=e*tan(E)-log(tan(E/2+pi/4));
1114 end
1115     
1116 
1117 
1118 
1119 %--------------------------------------------------------------------------
1120 function [r,v]=CUSTOMeph(jd,epoch,keplerian,flag)
1121 %
1122 %Returns the position and the velocity of an object having keplerian
1123 %parameters epoch,a,e,i,W,w,M
1124 %
1125 %Usage:     [r,v]=CUSTOMeph(jd,name,list,data,flag)
1126 %
1127 %Inputs:    jd: julian date
1128 %           epoch: mjd when the object was observed (referred to M)
1129 %           keplerian: vector containing the keplerian orbital parameters
1130 %
1131 %Output:    r = object position with respect to the Sun (km if flag=1, AU otherwise)
1132 %           v = object velocity ( km/s if flag=1, AU/days otherways )
1133 %
1134 %Revisions :    Function added 04/07
1135 
1136 global AU mu
1137 
1138 muSUN = mu(11);
1139      a=keplerian(1)*AU; %in km
1140      e=keplerian(2);
1141      i=keplerian(3); 
1142      W=keplerian(4);
1143      w=keplerian(5);
1144      M=keplerian(6);
1145      jdepoch=mjd2jed(epoch);
1146      DT=(jd-jdepoch)*60*60*24;
1147      n=sqrt(muSUN/a^3);
1148      M=M/180*pi;
1149      M=M+n*DT;
1150      M=mod(M,2*pi);
1151      E=M2E(M,e);
1152      [r,v]=par2IC([a,e,i/180*pi,W/180*pi,w/180*pi,E],muSUN);
1153      if flag~=1
1154          r=r/AU;
1155          v=v*86400/AU;
1156      end
1157  
1158 
1159 %--------------------------------------------------------------------------
1160 function t = time2distance(r0,v0,rtarget)
1161 %
1162 %Usage: t = time2distance(r0,v0,rtarget)
1163 %
1164 %Inputs:
1165 %           r0:    column vector for the position (mu=1)
1166 %           v0:    column vector for the velocity (mu=1)
1167 %           rtarget: distance to be reached
1168 %
1169 %Outputs:
1170 %           t:     time taken to reach a given distance
1171 %
1172 %Comments:  everything works in non dimensional units
1173 
1174 r0norm = norm(r0);
1175 if r0norm < rtarget
1176     out = sign(r0'*v0);
1177     E = IC2par(r0,v0,1);
1178     a = E(1); e = E(2); E0 = E(6); p = a * (1-e^2);
1179     %If the solution is an ellipse
1180     if e<1
1181         ra = a * (1+e);
1182         if rtarget>ra
1183             t = NaN;
1184         else %we find the anomaly where the target distance is reached
1185             ni = acos((p/rtarget-1)/e);         %in 0-pi
1186             Et = ni2E(ni,e);          %in 0-pi
1187             if out==1
1188                 t = a^(3/2)*(Et-e*sin(Et)-E0 +e*sin(E0));
1189             else
1190                 E0 = -E0;
1191                 t = a^(3/2)*(Et-e*sin(Et)+E0 - e*sin(E0));
1192             end
1193         end
1194     else %the solution is a hyperbolae
1195         ni = acos((p/rtarget-1)/e);         %in 0-pi
1196         Et = ni2E(ni,e);          %in 0-pi
1197         if out==1
1198                 t = (-a)^(3/2)*(e*tan(Et)-log(tan(Et/2+pi/4))-e*tan(E0)+log(tan(E0/2+pi/4)));
1199             else
1200                 E0 = -E0;
1201                 t = (-a)^(3/2)*(e*tan(Et)-log(tan(Et/2+pi/4))+e*tan(E0)-log(tan(E0/2+pi/4)));
1202         end
1203     end
1204 else
1205     t=12;
1206 end
1207     
1208 
1209 %--------------------------------------------------------------------------
1210 function jd = mjd20002jed(mjd2000)
1211 %This function converts mean julian date 2000 to julian date
1212 jd=mjd2000+2451544.5;
1213 
1214 
1215 %--------------------------------------------------------------------------
1216 function jd = mjd2jed(mjd)
1217 % This function converts mean Julian date into Julian date
1218 jd = mjd +2400000.5;
1219
mga_dsm

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SUBFUNCTIONS

SOURCE CODE
