Beispiel #1
0
  def invE(self,offsetArcLength=None,arcLengthArg=None,rtnIter=False):
    """Find eccentric anomaly corresponding to the input arc length"""

    ### Parse arguments
    assert not (offsetArcLength is not None and arcLengthArg is not None)
    if arcLengthArg is None: arcLengthTarget = self.arcChf + offsetArcLength
    else                   : arcLengthTarget = float(arcLengthArg)

    ### Initial estimate of Eccentric Anomaly
    ### - highest positive or negative multiple of halfpi that gives
    ###   an arc length less than or equal to targetLength
    nQuad = int(arcLengthTarget / self.quadArc)
    arcLengthGuess = nQuad * self.quadArc
    if arcLengthGuess > arcLengthTarget:
      nQuad -= 1
      arcLengthGuess -= self.quadArc
    eaGuess0 = eaGuess = nQuad * halfpi
    eaGuess0PlusTwoPi = eaGuess0 + twopi

    deltaArc =  arcLengthTarget - arcLengthGuess
    tol = self.aChf * 1e-9
    itter = 0
    while abs(deltaArc) > tol:

      itter += 1
      ### Vector from primary focus to Ecc. Anom. guess
      cosea = math.cos(eaGuess)
      sinea = math.sin(eaGuess)
      x = self.aChf * cosea
      y = self.bChf * sinea

      ### Delta-[x,y] of tangent at (x,y) for distance of deltaArc
      delx = -self.aChf * sinea
      dely =  self.bChf * cosea
      fac = deltaArc / sp.vnorm(sp.vpack(delx,dely,0.))
      delx *= fac
      dely *= fac
   
      ### Delta-vector tangent at (x,y) for distance of deltaArc
      ### - [X,Y,Z] = [x+delx, y+dely, 0]
      ### - scale X by 1/a and Y by 1/b to get Eccentric Anomaly
      eaGuess = sp.recrad(sp.vpack((x+delx)/self.aChf
                                   ,(y+dely)/self.bChf
                                   ,0.
                                   )
                          )[iRA]
      while eaGuess0 > eaGuess: eaGuess += twopi
      while (eaGuess0PlusTwoPi) <= eaGuess: eaGuess -= twopi
      arcLengthGuess = self.E(eaGuess)
      deltaArc = arcLengthTarget - arcLengthGuess

    return rtnIter and (eaGuess,itter,) or eaGuess
Beispiel #2
0
  def TrueToEccAnom(self,ta):
    """Convert True Anomaly to Eccentric Anomaly

https://en.wikipedia.org/wiki/Eccentric_anomaly#From_the_true_anomaly

"""
    return sp.recrad(sp.vpack(self.eccChf + math.cos(ta)
                             ,self.bOverA * math.sin(ta)
                             ,0.
                             )
                    )[iRA]
Beispiel #3
0
 def XYtoTrueAnom(self,x,y):
   return sp.recrad(sp.vpack(x-self.cChf,y,0.))[iRA]
Beispiel #4
0
  def inverseDeputy(self,stateDepEqcm=False):
    """Invert depyty-based calculations.  Chief-only calculations were
performed in __init__() method above
"""

    ####################################################################
    ### Process argmument ...
    if stateDepEqcm:
      ### Use deputy state argument (6-vector), if supplied
      rDepEqcm = stateDepEqcm[:3]
      velDepEqcm = stateDepEqcm[3:]
    else:
      ### Use result of .Deputy() method if no state is supplied
      rDepEqcm = self.rDepEqcm
      velDepEqcm = self.velDepEqcm

    ####################################################################
    ### Equations (11 and 12) are already done in Equations 1 through 5
    ### in the constructor __init__() above
    ### - .mtxEciToRsw1
    ### - .{r,vel}ChfRsw1
    ### - .lambdaPerigee
    ### - .trueAnom1

    ####################################################################
    ### Equations (13)
    ### in the constructor __init__() above
    ### - .mtxEciToRsw1

    self.zinvArcDep = self.arcChf + rDepEqcm[iAeq]
    self.zinvEccAnom2 = self.invE(arcLengthArg=self.zinvArcDep)

    self.zinvTrueAnom2 = self.XYtoTrueAnom(self.aChf * math.cos(self.zinvEccAnom2)
                                          ,self.bChf * math.sin(self.zinvEccAnom2)
                                          )

    ####################################################################
    ### Equation (14)

    self.zinvDeltaLambdaDep = (twopi + self.zinvTrueAnom2 - self.trueAnom1) % twopi

    ####################################################################
    ### Equations (6) repeat from Deputy method, as 2 may be different
    ### Equivalent chief position at point 2, using PQW2


    rChf2 = self.pChf / (1. + (self.eccChf * math.cos(self.zinvTrueAnom2)))
    pHat = self.vEccHatChf
    qHat = sp.ucrss([0.,0.,1.],pHat)
    self.zinvRChfPqw2 = sp.vscl(rChf2,sp.vadd(sp.vscl(math.cos(self.zinvTrueAnom2),pHat),sp.vscl(math.sin(self.zinvTrueAnom2),qHat)))

    self.zinvVelChfPqw2 = sp.vscl(math.sqrt(self.mu/self.pChf)
                                 ,sp.vadd(sp.vscl(           -math.sin(self.zinvTrueAnom2),pHat)
                                         ,sp.vscl(self.eccChf+math.cos(self.zinvTrueAnom2),qHat)
                                         )
                                 )

    ####################################################################
    ### Equations (7) repeat from Deputy method, as 2 may be different
    ### Convert from PQW2 to RSW2
    self.zinvMtxPqw2toRsw2 = RVtoRSW(self.zinvRChfPqw2,self.zinvVelChfPqw2)
    self.zinvRChfRsw2 = sp.mxv(self.zinvMtxPqw2toRsw2,self.zinvRChfPqw2)
    self.zinvVelChfRsw2 = sp.mxv(self.zinvMtxPqw2toRsw2,self.zinvVelChfPqw2)

    ####################################################################
    ### Equations (15)
    self.zinvDeltaPhiDep = rDepEqcm[iZeq] / rChf2

    rDepHatRsw1 = sp.radrec(1., self.zinvDeltaLambdaDep, self.zinvDeltaPhiDep)

    ####################################################################
    ### - Equations (8) repeat from Deputy method, as deputy may differ
    ###   - Transformation matrix from RSW to SEZ frame
    ###   - Matrix is ROT2[90-deltaPhiDep] ROT3[deltaLambdaDep]
    self.zinvMtxRswToSez = sp.eul2m(halfpi-self.zinvDeltaPhiDep,self.zinvDeltaLambdaDep,0.,2,3,1)

    ####################################################################
    ### Equations (16)
    ### - Transform deputy unit vector from RSW1 to SEZ
    ### - Scale deputy unit vectors in RSW1 and in SEZ
    ###   - factor is X (R) component of deputy EQCM X (R), plus R
    ###       component of chief RSW2
    ### - Transform deputy vector from RSW1 to ECI (transpose matrix)
    rDepHatSez = sp.mxv(self.zinvMtxRswToSez,rDepHatRsw1)
    sezScale = (rDepEqcm[iReq] + self.zinvRChfRsw2[iRrsw]) / rDepHatSez[iZsez]
    self.zinvRDepSez = sp.vscl(sezScale, rDepHatSez)
    self.zinvRDepRsw1 = sp.vscl(sezScale, rDepHatRsw1)
    self.zinvRDepEci = sp.mtxv(self.mtxEciToRsw1, self.zinvRDepRsw1)

    ####################################################################
    ### Equations (17)
    ### - final velocity displacements
    ### - Not working yet:  what is rChf?
    self.zinvVelDepSez = sp.vpack(-velDepEqcm[iZeq] * sezScale / rChf2
                                #,(velDepEqcm[iAeq] + self.velChfRsw1[iSrsw]) * sezScale *          self.zinvDeltaPhiDep  / rChf2
                                 ,(velDepEqcm[iAeq] + self.velChfRsw1[iSrsw]) * sezScale * math.cos(self.zinvDeltaPhiDep) / rChf2
                                 ,velDepEqcm[iReq] + self.zinvVelChfRsw2[iRrsw]
                                 )
    self.zinvVelDepRsw1 = sp.mtxv(self.zinvMtxRswToSez, self.zinvVelDepSez)
    self.zinvVelDepEci = sp.mtxv(self.mtxEciToRsw1, self.zinvVelDepRsw1)

    return self.zinvRDepEci,self.zinvVelDepEci
Beispiel #5
0
  def Deputy(self,stateDep):
    """Make depyty-based calculations.  Chief-only calculations were
performed in __init__() method above

"""
    ####################################################################
    ### Extract Deputy ECI Position and velocity from 6-element deputy
    ###   ECI state
    self.rDepEci,self.velDepEci = stateDep[:3], stateDep[3:]

    ####################################################################
    ### Equations (1) and (2)
    ### Convert deputy position and velocity to RSW using
    ###   [Rhat|Shat|What]1 matrix
    (self.rDepRsw1
    ,self.velDepRsw1
    ) = [ sp.mxv(self.mtxEciToRsw1,v)
          for v in 
          [self.rDepEci,self.velDepEci]
        ]

    ####################################################################
    ### Equations (3)
    ### Get delta-lambda to deputy as RA from (Radius,RA,DEC)
    ###   returned by recrad.
    RrdDepRsw1 = sp.recrad(self.rDepRsw1)
    self.deltaLambdaDep = sp.recrad(self.rDepRsw1)[iRA]

    ####################################################################
    ### Equations (5)
    ### Deputy True anomaly
    self.trueAnom2 = (twopi + self.deltaLambdaDep - self.lambdaPerigee) % twopi

    ####################################################################
    ### Equations (6)
    ### Equivalent chief position at point 2, using PQW2
    rChf2 = self.pChf / (1. + (self.eccChf * math.cos(self.trueAnom2)))
    pHat = self.vEccHatChf
    qHat = sp.ucrss([0.,0.,1.],pHat)
    self.rChfPqw2 = sp.vscl(rChf2,sp.vadd(sp.vscl(math.cos(self.trueAnom2),pHat),sp.vscl(math.sin(self.trueAnom2),qHat)))
    self.velChfPqw2 = sp.vscl(math.sqrt(self.mu/self.pChf)
                             ,sp.vadd(sp.vscl(           -math.sin(self.trueAnom2),pHat)
                                     ,sp.vscl(self.eccChf+math.cos(self.trueAnom2),qHat)
                                     )
                             )

    ####################################################################
    ### Equations (7)
    ### Convert from PQW2 to RSW2
    self.mtxPqw2toRsw2 = RVtoRSW(self.rChfPqw2,self.velChfPqw2)
    self.rChfRsw2 = sp.mxv(self.mtxPqw2toRsw2,self.rChfPqw2)
    self.velChfRsw2 = sp.mxv(self.mtxPqw2toRsw2,self.velChfPqw2)

    ####################################################################
    ### Equations (8)
    ### Transform Deputy vectors to SEZ frame
    ### - deltaPhiDep is DEC from [Radius,RA,DEC] of rDepRsw1
    ### - Matrix is ROT2[90-deltaPhiDep] ROT3[deltaLambdaDep]
    self.deltaPhiDep = RrdDepRsw1[iDEC]
    self.mtxRswToSez = sp.eul2m(halfpi-self.deltaPhiDep,self.deltaLambdaDep,0.,2,3,1)
    self.rDepSez = sp.mxv(self.mtxRswToSez,self.rDepRsw1)
    self.velDepSez = sp.mxv(self.mtxRswToSez,self.velDepRsw1)

    ####################################################################
    ### Equations (9)
    ### Transform Deputy vectors from SEZ to EQCM frame

    ### 9.1) Convert True Anomalies 2 (Deputy) to Eccentric Anomaly 2
    ### - https://en.wikipedia.org/wiki/Eccentric_anomaly#From_the_true_anomaly
    ### - Possible exceptions:  divBy0; domain error.
    ### - Chief done in __init__() method above
    self.eccAnom2 = self.TrueToEccAnom(self.trueAnom2)

    ### Ensure .eccAnom2 is within PI/2 of .eccAnom1
    while self.eccAnom2 >  (self.eccAnom1+math.pi): self.eccAnom2 -= (2 * math.pi)
    while self.eccAnom2 <= (self.eccAnom1-math.pi): self.eccAnom2 += (2 * math.pi)

    ### 9.2) Get arc length from positive semi-major axis to deputy
    self.arcDep = self.E(self.eccAnom2)

    ### 9.3) Relate the deputy relative to chief at point 2
    self.rDepEqcm = sp.vpack( self.rDepSez[iZsez] - self.rChfRsw2[iRrsw]
                            , self.arcDep - self.arcChf
                            , self.deltaPhiDep * rChf2
                            )
    self.velDepEqcm = sp.vpack( self.velDepSez[iZsez] - self.velChfRsw2[iRrsw]
                             #, (self.velDepSez[iE] * rChf2 / (RrdDepRsw1[iRadius] *          self.deltaPhiDep )) - self.velChfRsw1[iE]
                              , (self.velDepSez[iE] * rChf2 / (RrdDepRsw1[iRadius] * math.cos(self.deltaPhiDep))) - self.velChfRsw1[iE]
                              , -self.velDepSez[iSsez] * rChf2 / RrdDepRsw1[iRadius]
                              )

    return self.rDepEqcm, self.velDepEqcm