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
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
