state,lt = spice.spkezr(spacecraft,et,'J2000','NONE',target)
dt = spice.vdot(state[:3],state[3:]) / spice.vdot(state[3:],state[3:])
oldet = et
et = et - dt
while abs(et-oldet)>0.:
state,lt = spice.spkezr(spacecraft,et,'J2000','NONE',target)
dt = spice.vdot(state[:3],state[3:]) / spice.vdot(state[3:],state[3:])
oldet = et
et = et - dt
utcPRG = spice.et2utc(et,'ISOC',3,99)
mtxJ2kToBF = spice.tipbod('J2000', targetID, et)
velocityPRG_BF = spice.mxv(mtxJ2kToBF,state[3:])
V_prg,ra,Theta = spice.recrad(velocityPRG_BF)
Theta_deg = Theta * dpr
print((dt,dt/et,utcPRG,V_prg,Theta_deg))
import matplotlib.pyplot as plt
days = 7
hours = xrange(-24*days,24*days + 1)
speeds = [ spice.vnorm(spice.spkezr(spacecraft,et+(hour*3600.),'J2000','NONE',target)[0][3:]) for hour in hours]
Vmin = min(speeds)
plt.axhline(y=V_prg,label='V_prg = %.3fkm/s' % (V_prg,))
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 __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)
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
