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