Exemplo n.º 1
0
 def initFromStateVectors(self,epoch,pV,vV):
     self.epoch = epoch
     
     # 1) Calculate auxilary vector h
     hV = cross(pV,vV)
     
     
     # 2) Normalize position,velocity, specific angular momentum, calculate radial velocity 
     
     p = linalg.norm(pV)
     v = linalg.norm(vV)
     h = linalg.norm(hV)
     print "H:",h
     radv = pV.dot(vV) / p
     hVu = hV / h
     pVu = pV / p
     nV = cross(array([0,0,1]),hV)
     n = linalg.norm(nV)
     if n == 0:
         nVu = array([0,0,0])
     else:
         nVu = nV/n
     # 3) Calculate inclination
     #self.i = arccos(hV[2]/h)
     self.i = arcsin(linalg.norm(cross(array([0,0,1]),hVu)))
     print "i1",self.i
     print "RADVEL",radv
     self.i = arccos(array([0,0,1]).dot(hV)/h)
     #if radv < 0:
     #    self.i = PI2 - self.i 
     print "i2",self.i
     # 4) Calculate node line
     
     
     # 5) Calculate longitude of ascending node = right ascension of ascending node
     '''
     if self.i == 0:
         self.lan=0
     elif nV[1] >= 0:
         self.lan = arccos(nV[0] / n)
     else:
         self.lan = PI2 - arccos(nV[0] / n)
     '''
     
     if self.i == 0:
         self.lan = 0
     else:
         self.lan = arcsin(cross(array([1,0,0]),nVu).dot(array([0,0,1])))
         print "lan1",self.lan
         self.lan = arccos(array([1,0,0]).dot(nV)/n)
         if nV[1] < 0:
             self.lan = PI2-self.lan
         print "lan2",self.lan
     
     # 6) Eccentricity vector
     #eV = (1.0 / self.ref.mu)*((v**2 - (self.ref.mu / p))*pV - radv*vV)
     #eV2 = (1.0 / self.ref.mu) * ( hV - self.ref.mu * (pV/p))
     #eV3 = hV/self.ref.mu - (pV/p)
     
     # Source: cdeagle
     eV = cross(vV,hV)/self.ref.mu - pVu
     #print "eV1:",eV,linalg.norm(eV)
     #print "eV2:",eV2,linalg.norm(eV2)
     #print "eV3:",eV3,linalg.norm(eV3)
     print "eV3:",eV,linalg.norm(eV)
     self._e = linalg.norm(eV)
     #eVu = eV / self.e
     
     print "h",h
     print "u",self.ref.mu
     print "v",v
     print "r",p
     
     print "alte:",sqrt(1+(h**2/self.ref.mu**2)*(v**2-(2*self.ref.mu)/p)**2)
     # 7) Argument of perigree
     '''
     if self.e == 0:
         self.aop = 0
     elif self.i == 0:
       self.aop = arccos(eV[0] / self.e)  
     elif eV[2] >= 0:
         print "AOP AOP AOP"
         #self.aop = arccos(nV.dot(eV) / (n*self.e))
         print cross(nV,eV).dot(hV)
         self.aop = arcsin(cross(nVu,eVu).dot(hVu))
         #self.aop = arccos(n*self.e)
     else:
         self.aop = PI2 - arccos(nV.dot(eV) / (n*self.e))
     '''
     #CDEagle method
     # TODO CHECK how KSP handles this. 
     if self.e == 0:
         self.aop = 0
     elif self.i == 0 and self.e != 0:            
         #self.aop = arccos(eV[0] / self.e)
         #self.aop = arctan2(eV[1],eV[0])
         self.aop = arccos(array([1,0,0]).dot(eV) / self.e)
         print eV
         if eV[2] < 0:
             #self.aop = -self.aop
             self.aop = PI2-self.aop
         
         #print "BOOM",eV
         #if eV[2] < 0:
         #    print "BAM N***A"
         #    self.aop = PI2 - self.aop
     elif self.i == 0 and self.e == 0:
         #raise AttributeError("Perfectly circular orbits are not supported atm")
         self.aop = 0
     else:
         #self.aop = arcsin(cross(nVu,eVu).dot(hVu))
         self.aop = arccos(nV.dot(eV)/(n*self.e))
         if eV[2] < 0:
             self.aop = PI2-self.aop
     
     # 8) Semi major axis
     aE = v**2/2.0 - self.ref.mu / p
     self._a = -self.ref.mu / (2*aE)
     print "Old method for semi-major",self.a
     self._a = h**2 / (self.ref.mu * (1-self.e**2))
     print "New method for semi-major",self.a  
     #if self.e > 1:
     #    self._a = h**2 / (self.ref.mu * (self.e**2 - 1))
     
     if self.e == 0:
         if self.i == 0: #TODO update document to this
             print "JEA JEA JEA JEA"*10
             
             ta = arccos(array([1,0,0]).dot(pV) / p)
             if pV[1] < 0: # Vallado pg. 111
                 ta = PI2 - ta
         else: #TODO VERIFY THIS CASE
             ta = arccos((nV.dot(pV))/(n*p))
             if pV[2] < 0: # Vallado pg. 110
                 ta = PI2 - ta
         E = ta
         self.M0 = E
           
     elif self.e < 1:
         # 9) True anomaly, eccentric anomaly and mean anomaly
         if radv >= 0:
             ta = arccos((eV / self.e).dot(pV/p))
         else:
             ta = PI2 - arccos((eV / self.e).dot(pV/p))
         
         
         E = arccos((self.e+cos(ta))/(1+ self.e*cos(ta)))
         if radv < 0:
             E = PI2 - E
     
         self.M0 = E - self.e * sin(E)
         
     elif self.e > 1:
         # 9) Hyperbolic True anomaly, eccentric anomaly and mean anomaly
         # http://scienceworld.wolfram.com/physics/HyperbolicOrbit.html
         V = arccos((abs(self.a)*(self.e**2 - 1)) /(self.e * p) - 1/self.e)
         ta = arccos((eV / self.e).dot(pV/p))
         
         if radv < 0: #TODO: Should affect F too?
             # Negative = heading towards periapsis
             print "PI2"
             V = PI2 - V
             ta = PI2-ta
         print "V",V
         print "TA",ta
         # http://www.bogan.ca/orbits/kepler/orbteqtn.html In you I trust
         # Hyperbolic eccentric anomaly
         cosV = cos(V)
         F = arccosh((self.e+cosV)/(1+self.e*cosV))
         if radv < 0:
             F = -F
         F2 = arcsinh((sqrt(self.e-1)*sin(V))/(1+self.e*cos(V)))
         ##F1 = F2
         print "F1:",F
         print "F2:",F2
         self.M0 = self.e * sinh(F) - F
         
     
  
     
     
     self.h = h
     
     print "Semi-major:",self.a
     print "Eccentricity:",self.e
     print "Inclination:",degrees(self.i),"deg"
     print "LAN:",degrees(self.lan),"deg"
     print "AoP:",degrees(self.aop),"deg"
     print "Mean anomaly:",self.M0
     print "Specific angular momentum:",self.h
     if self.e < 1:
         print "Eccentric anomaly",E
         print "True anomaly",ta
     else:
         print "Hyperbolic eccentric anomaly",F
         print "Hyperbolic true anomaly",degrees(V)
         
     print "Distance from object:",p
     print "Velocity:",v
Exemplo n.º 2
0
def lambert(r1vec,r2vec,tf,m,muC):
# original documentation:
# ·············································
#
# This routine implements a new algorithm that solves Lambert's problem. The
# algorithm has two major characteristics that makes it favorable to other
# existing ones.
#
# 1) It describes the generic orbit solution of the boundary condition
# problem through the variable X=log(1+cos(alpha/2)). By doing so the
# graph of the time of flight become defined in the entire real axis and
# resembles a straight line. Convergence is granted within few iterations
# for all the possible geometries (except, of course, when the transfer
# angle is zero). When multiple revolutions are considered the variable is
# X=tan(cos(alpha/2)*pi/2).
#
# 2) Once the orbit has been determined in the plane, this routine
# evaluates the velocity vectors at the two points in a way that is not
# singular for the transfer angle approaching to pi (Lagrange coefficient
# based methods are numerically not well suited for this purpose).
#
# As a result Lambert's problem is solved (with multiple revolutions
# being accounted for) with the same computational effort for all
# possible geometries. The case of near 180 transfers is also solved
# efficiently.
#
#  We note here that even when the transfer angle is exactly equal to pi
# the algorithm does solve the problem in the plane (it finds X), but it
# is not able to evaluate the plane in which the orbit lies. A solution
# to this would be to provide the direction of the plane containing the
# transfer orbit from outside. This has not been implemented in this
# routine since such a direction would depend on which application the
# transfer is going to be used in.
#
# please report bugs to [email protected]    
#
# adjusted documentation:
# ·······················
#
# By default, the short-way solution is computed. The long way solution
# may be requested by giving a negative value to the corresponding
# time-of-flight [tf].
#
# For problems with |m| > 0, there are generally two solutions. By
# default, the right branch solution will be returned. The left branch
# may be requested by giving a negative value to the corresponding
# number of complete revolutions [m].

# Authors
# .·`·.·`·.·`·.·`·.·`·.·`·.·`·.·`·.·`·.·`·.·`·.·`·.·`·.·`·.·`·.
# Name       : Dr. Dario Izzo
# E-mail     : [email protected]
# Affiliation: ESA / Advanced Concepts Team (ACT)

# Made readible and optimized for speed by Rody P.S. Oldenhuis
# Code available in MGA.M on   http://www.esa.int/gsp/ACT/inf/op/globopt.htm

# last edited 12/Dec/2009

# ADJUSTED FOR EML-COMPILATION 24/Dec/2009
    
    # initial values        
    tol = 1e-12
    bad = False
    days = 1

    # work with non-dimensional units
    r1 = norm(r1vec) #sqrt(r1vec*r1vec.');  r1vec = r1vec/r1;
    r1vec = r1vec / r1
    r2vec = r2vec / r1
    
    V = sqrt(muC/r1)     
    T = r1/V              
    tf= tf*days/T # also transform to seconds

    # relevant geometry parameters (non dimensional)
    mr2vec = norm(r2vec)
    # make 100# sure it's in (-1 <= dth <= +1)
    dth = arccos( max(-1, min(1, (r1vec.dot(r2vec)/mr2vec))))  
    
        
    # decide whether to use the left or right branch (for multi-revolution
    # problems), and the long- or short way    
    leftbranch = sign(m)
    longway = sign(tf)
    m = abs(m)
    tf = abs(tf)
    if (longway < 0):
        dth = 2*pi - dth  

    # derived quantities        
    c      = sqrt(1.0 + mr2vec**2 - 2*mr2vec*cos(dth)) # non-dimensional chord
    s      = (1.0 + mr2vec + c)/2.0                    # non-dimensional semi-perimeter
    a_min  = s/2.0                              # minimum energy ellipse semi major axis
    Lambda = sqrt(mr2vec)*cos(dth/2.0)/s              # lambda parameter (from BATTIN's book)

    crossprd = cross(r1vec,r2vec)

    mcr       = norm(crossprd)           # magnitues thereof
    nrmunit   = crossprd/mcr                        # unit vector thereof
    
    # Initial values
    # ·························································

    # ELMEX requires this variable to be declared OUTSIDE the IF-statement
    logt = log(tf); # avoid re-computing the same value
    
    # single revolution (1 solution)
    if (m == 0):

        # initial values        
        inn1 = -0.5233      # first initial guess
        inn2 = +0.5233      # second initial guess
        x1   = log(1 + inn1)# transformed first initial guess
        x2   = log(1 + inn2)# transformed first second guess

        # multiple revolutions (0, 1 or 2 solutions)
        # the returned soltuion depends on the sign of [m]
    else:          
        # select initial values
        if (leftbranch < 0):
            inn1 = -0.5234 # first initial guess, left branch
            inn2 = -0.2234 # second initial guess, left branch
        else:
            inn1 = +0.7234 # first initial guess, right branch
            inn2 = +0.5234 # second initial guess, right branch
        x1 = tan(inn1*pi/2)# transformed first initial guess
        x2 = tan(inn2*pi/2)# transformed first second guess
    

    # since (inn1, inn2) < 0, initial estimate is always ellipse
    xx   = array([inn1, inn2])
    aa = a_min/(1 - xx**2)
    
    bbeta = longway * 2*arcsin(sqrt((s-c)/2./aa))
    # make 100.4% sure it's in (-1 <= xx <= +1)
    if xx[0] > 1: xx[0] = 1
    if xx[0] < -1: xx[0] = -1
    if xx[1] > 1: xx[1] = 1
    if xx[1] < -1: xx[1] = -1
    aalfa = 2*arccos( xx )

    # evaluate the time of flight via Lagrange expression
    y12  = aa*sqrt(aa)*((aalfa - sin(aalfa)) - (bbeta-sin(bbeta)) + 2*pi*m)

    # initial estimates for y
    if m == 0:
        y1 = log(y12[0]) - logt
        y2 = log(y12[1]) - logt
    else:
        y1 = y12[0] - tf
        y2 = y12[1] - tf
    

    # Solve for x
    # ·························································
    
    # Newton-Raphson iterations
    # NOTE - the number of iterations will go to infinity in case
    # m > 0  and there is no solution. Start the other routine in 
    # that case
    
    err = 1e99  
    iterations = 0
    xnew = 0
    while (err > tol):
        # increment number of iterations
        iterations += 1
        # new x
        xnew = (x1*y2 - y1*x2) / (y2-y1);
        # copy-pasted code (for performance)
        if m == 0:
            x = exp(xnew) - 1
        else:
            x = arctan(xnew)*2/pi
            
        a = a_min/(1 - x**2);
        if (x < 1): # ellipse
            beta = longway * 2*arcsin(sqrt((s-c)/2/a))
            # make 100.4% sure it's in (-1 <= xx <= +1)
            alfa = 2*arccos( max(-1, min(1, x)) )
        else: # hyperbola
            alfa = 2*arccosh(x);
            beta = longway * 2*arcsinh(sqrt((s-c)/(-2*a)))
        
        # evaluate the time of flight via Lagrange expression
        if (a > 0):
            tof = a*sqrt(a)*((alfa - sin(alfa)) - (beta-sin(beta)) + 2*pi*m)
        else:
            tof = -a*sqrt(-a)*((sinh(alfa) - alfa) - (sinh(beta) - beta))
        
        # new value of y
        if m ==0:
            ynew = log(tof) - logt
        else:
            ynew = tof - tf
        # save previous and current values for the next iterarion
        # (prevents getting stuck between two values)
        x1 = x2;  x2 = xnew;
        y1 = y2;  y2 = ynew;
        # update error
        err = abs(x1 - xnew);
        # escape clause
        if (iterations > 15):
            bad = True 
            break
    
    
  
    
    # If the Newton-Raphson scheme failed, try to solve the problem
    # with the other Lambert targeter. 
    if bad:
        # NOTE: use the original, UN-normalized quantities
        #[V1, V2, extremal_distances, exitflag] = ...
        #    lambert_high_LancasterBlanchard(r1vec*r1, r2vec*r1, longway*tf*T, leftbranch*m, muC);
        print "FAILZ0r"
        return
    
    
    # convert converged value of x
    if m==0:
        x = exp(xnew) - 1
    else:
        x = arctan(xnew)*2/pi
    
    #{
    #      The solution has been evaluated in terms of log(x+1) or tan(x*pi/2), we
    #      now need the conic. As for transfer angles near to pi the Lagrange-
    #      coefficients technique goes singular (dg approaches a zero/zero that is
    #      numerically bad) we here use a different technique for those cases. When
    #      the transfer angle is exactly equal to pi, then the ih unit vector is not
    #      determined. The remaining equations, though, are still valid.
    #}

    # Solution for the semi-major axis
    a = a_min/(1-x**2);

    # Calculate psi
    if (x < 1): # ellipse
        beta = longway * 2*arcsin(sqrt((s-c)/2/a))
        # make 100.4# sure it's in (-1 <= xx <= +1)
        alfa = 2*arccos( max(-1, min(1, x)) )
        psi  = (alfa-beta)/2
        eta2 = 2*a*sin(psi)**2/s
        eta  = sqrt(eta2);
    else:       # hyperbola
        beta = longway * 2*arcsinh(sqrt((c-s)/2/a))
        alfa = 2*arccosh(x)
        psi  = (alfa-beta)/2
        eta2 = -2*a*sinh(psi)**2/s
        eta  = sqrt(eta2)
    

    # unit of the normalized normal vector
    ih = longway * nrmunit;

    # unit vector for normalized [r2vec]
    r2n = r2vec/mr2vec;

    # cross-products
    # don't use cross() (emlmex() would try to compile it, and this way it
    # also does not create any additional overhead)
    #crsprd1 = [ih(2)*r1vec(3)-ih(3)*r1vec(2),...
    #           ih(3)*r1vec(1)-ih(1)*r1vec(3),...
    #           ih(1)*r1vec(2)-ih(2)*r1vec(1)];    
    crsprd1 = cross(ih,r1vec)
    #crsprd2 = [ih(2)*r2n(3)-ih(3)*r2n(2),...
    #           ih(3)*r2n(1)-ih(1)*r2n(3),...
    #           ih(1)*r2n(2)-ih(2)*r2n(1)];
    crsprd2 = cross(ih,r2n)

    # radial and tangential directions for departure velocity
    Vr1 = 1/eta/sqrt(a_min) * (2*Lambda*a_min - Lambda - x*eta)
    Vt1 = sqrt(mr2vec/a_min/eta2 * sin(dth/2)**2)

    # radial and tangential directions for arrival velocity
    Vt2 = Vt1/mr2vec
    Vr2 = (Vt1 - Vt2)/tan(dth/2) - Vr1
    
    # terminal velocities
    V1 = (Vr1*r1vec + Vt1*crsprd1)*V
    V2 = (Vr2*r2n + Vt2*crsprd2)*V
    
    # exitflag
    #exitflag = 1 # (success)
    #print "V1:",V1
    #print "V2:",V2
    return V1,V2