Home > Mateda2.0 > functions > trajectory > mga_dsm.m

# mga_dsm

## PURPOSE

------------------------------------------------------------------------

## SYNOPSIS

function [J,DVvec] = mga_dsm(t,problem)

## DESCRIPTION

     ------------------------------------------------------------------------
     This source file is part of the 'ESA Advanced Concepts Team's
     Space Mechanics Toolbox' software.

     The source files are for research use only,
     and are distributed WITHOUT ANY WARRANTY. Use them on your own risk.

     Copyright (c) 2004-2007 European Space Agency
     ------------------------------------------------------------------------

## CROSS-REFERENCE INFORMATION

This function calls:

This function is called by:

- EvalSaga

## SUBFUNCTIONS

- function [r,v] = propagateKEP(r0,v0,t,mu)
- function E=IC2par(r0,v0,mu)
- function E=ni2E(ni,e)
- function [r0,v0]=par2IC(E,mu)
- function [v1,v2,a,p,theta,iter]=lambertI(r1,r2,t,mu,lw,N,branch)
- function t=x2tof(x,s,c,lw,N)
- function t=tofabn(sigma,alfa,beta,N)
- function v=vers(V)
- function ansd = vett(r1,r2)
- function [r,v,E]=pleph_an ( mjd2000, planet);
- function [r,v] = conversion (E)
- function E=M2E(M,e)
- function M=E2M(E,e)
- function [r,v]=CUSTOMeph(jd,epoch,keplerian,flag)
- function t = time2distance(r0,v0,rtarget)
- function jd = mjd20002jed(mjd2000)
- function jd = mjd2jed(mjd)

## SOURCE CODE

    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