示例#1
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
示例#2
0
  def __init__(self,stateChf,mu,stateDep=None):
    """Make chief-only calculations.  Save deputy-based calculations for
Deputy() method below; call it now if stateDep is provided

mu = GM, km**3 s**-2

"""
    ####################################################################
    ### Extract ECI Positions and ECI velocities from 6-element ECI states
    ### Save mu in self object
    self.rChfEci,self.velChfEci = stateChf[:3], stateChf[3:]
    self.mu = mu

    ####################################################################
    ### Equations (1) and (2)
    ### Convert chief position and velocity to [Rhat|Shat|What]1 matrix
    self.mtxEciToRsw1 = RVtoRSW(self.rChfEci,self.velChfEci)

    ####################################################################
    ### Transform chief position and velocity to RSW1 frame
    (self.rChfRsw1
    ,self.velChfRsw1
    ) = [ sp.mxv(self.mtxEciToRsw1,v)
          for v in 
          [self.rChfEci,self.velChfEci]
        ]

    ####################################################################
    ### Equations (3) - postponed to deputy calculation

    ####################################################################
    ### Equations (4)
    ### Eccentricity and semi-major axis
    ### - vHChf = momentum vector
    ### - self.pChf = semiparameter or semilatus rectum = distance from
    ###               the primary focus to the orbit, measured
    ###               perpendicular to the semi-major axis
    ### - self.eccChfSquared = Square of Eccentricity
    ###   - derivation, dot product of vEccChf with itself, will always
    ###       be non-negative
    ###   - square root of (1-self.eccChfSquared) will throw math domain
    ###       exception if eccentricity exceeds unity.
    ###   - reciprocal of (1-self.eccChfSquared) will throw division by
    ###       zero exception if eccentricity is unity
    ### - self.eccChf = Eccentricity (also Elliptic Modulus, k)
    ### - self.bOverA = ratio of semi-minor to semi-major axes' lengths
    ### - self.aChf = length of semi-major axis
    ### - self.bChf = length of semi-minor axis
    vHChf = sp.vcrss(self.rChfRsw1,self.velChfRsw1)
    self.pChf = sp.vdot(vHChf,vHChf) / self.mu
    vEccChf = sp.vsub(sp.vscl(1./self.mu,sp.vcrss(self.velChfRsw1,vHChf)),sp.vhat(self.rChfRsw1))
    self.eccChfSquared = sp.vdot(vEccChf,vEccChf)
    self.eccChf = math.sqrt(self.eccChfSquared)
    self.bOverA = math.sqrt(1 - self.eccChfSquared)
    self.aChf = self.pChf / (1. - self.eccChfSquared)
    self.bChf = self.pChf / self.bOverA
    self.cChf = self.aChf * self.eccChf
    if self.eccChfSquared>0.: self.vEccHatChf = sp.vhat(vEccChf)
    else                    : self.vEccHatChf = sp.vhat(self.rChfRsw1)

    ### Use SPICE toolkit routine to calculate perfocal distance (rp), eccentricity, and semi-major axis
    self.rpChf,self.eccChfSpice,inc,lnode,argp,m0,t0,muTmp = sp.oscelt(stateChf,0.,self.mu)
    self.aChfSpice = self.rpChf / (1. - self.eccChfSpice)

    ### Quadrant arc length; used later
    self.quadArc = self.aChf * E(halfpi,self.eccChfSquared)

    ####################################################################
    ### Equations (5)
    ### Chief True anomaly posiiton
    ### Deputy True anomaly postponed to deputy calculation
    self.lambdaPerigee = sp.recrad(self.vEccHatChf)[iRA]
    self.trueAnom1 = (twopi - self.lambdaPerigee) % twopi

    ####################################################################
    ### Equations (6 through 9) - postponed to deputy calculation

    ####################################################################
    ### Equations (9) - chief only

    ### 9.1) Convert True Anomaly 1 (chief) to Eccentric Anomaly
    self.eccAnom1 = self.TrueToEccAnom(self.trueAnom1)

    ### Get the arc length from periapse to the chief
    self.arcChf = self.E(self.eccAnom1)

    ####################################################################
    ### Complete conversion to EQCM for Deputy if Deputy state is present

    if not (stateDep is None): self.Deputy(stateDep)
示例#3
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