def geo2rdr(self, llh, side=-1, planet=None, doppler=None, wvl=None):
'''
Takes a lat, lon, height triplet and returns azimuth time and range.
Assumes zero doppler for now.
'''
from isceobj.Planet.Planet import Planet
from isceobj.Util.Poly2D import Poly2D
if doppler is None:
doppler = Poly2D()
doppler.initPoly(azimuthOrder=0, rangeOrder=0, coeffs=[[0.]])
wvl = 0.0
if planet is None:
refElp = Planet(pname='Earth').ellipsoid
else:
refElp = planet.ellipsoid
xyz = refElp.llh_to_xyz(llh)
delta = (self.maxTime - self.minTime).total_seconds() * 0.5
tguess = self.minTime + datetime.timedelta(seconds=delta)
outOfBounds = False
# Start the previous guess tracking with dummy value
t_prev_guess = tguess + datetime.timedelta(seconds=10)
for ii in range(51):
try:
sv = self.interpolateOrbit(tguess, method='hermite')
except:
outOfBounds = True
break
pos = np.array(sv.getPosition())
vel = np.array(sv.getVelocity())
dr = xyz - pos
rng = np.linalg.norm(dr)
dopfact = np.dot(dr, vel)
fdop = doppler(DTU.seconds_since_midnight(tguess), rng) * wvl * 0.5
fdopder = (0.5 * wvl * doppler(DTU.seconds_since_midnight(tguess),
rng + 10.0) - fdop) / 10.0
fn = dopfact - fdop * rng
c1 = -np.dot(vel, vel)
c2 = (fdop / rng + fdopder)
fnprime = c1 + c2 * dopfact
tguess = tguess - datetime.timedelta(seconds=fn / fnprime)
if abs(tguess - t_prev_guess).total_seconds() < 5e-9:
break
else:
t_prev_guess = tguess
if outOfBounds:
raise Exception('Interpolation time out of bounds')
return tguess, rng
def testLATLON(self):
elp = Planet("Earth").get_elp()
#From for_ellipsoid_test.F
r_xyz = [7000000.0, -7500000.0, 8000000.0]
r_llh = [38.038207425428674, -46.974934010881981, 6639569.3697941694]
posLLH = elp.xyz_to_llh(r_xyz)
for (a, b) in zip(r_llh[:2], posLLH[:2]):
self.assertAlmostEqual(a, b, places=3)
self.assertAlmostEqual(r_llh[2], posLLH[2], delta=.1)
r_llh = [-33.0, 118.0, 2000.0]
r_xyz = [-2514561.1100611691, 4729201.6284226896, -3455047.9192480515]
posXYZ = elp.llh_to_xyz(r_llh)
for (a, b) in zip(r_xyz, posXYZ):
self.assertAlmostEqual(a, b, places=3)
def rdr2geoNew(self, aztime, rng, height=0.,
doppler = None, wvl = None,
planet=None, side=-1):
'''
Returns point on ground at given height and doppler frequency.
Never to be used for heavy duty computing.
'''
from isceobj.Planet.Planet import Planet
####Setup doppler for the equations
dopfact = 0.
sv = self.interpolateOrbit(aztime, method='hermite')
pos = np.array(sv.getPosition())
vel =np.array( sv.getVelocity())
vmag = np.linalg.norm(vel)
if doppler is not None:
dopfact = doppler(DTU.seconds_since_midnight(aztime), rng) * 0.5 * wvl * rng/vmag
if planet is None:
refElp = Planet(pname='Earth').ellipsoid
else:
refElp = planet.ellipsoid
###Convert position and velocity to local tangent plane
major = refElp.a
minor = major * np.sqrt(1 - refElp.e2)
#####Setup ortho normal system right below satellite
satDist = np.linalg.norm(pos)
alpha = 1 / np.linalg.norm(pos/ np.array([major, major, minor]))
radius = alpha * satDist
hgt = (1.0 - alpha) * satDist
###Setup TCN basis - Geocentric
nhat = -pos/satDist
temp = np.cross(nhat, vel)
chat = temp / np.linalg.norm(temp)
temp = np.cross(chat, nhat)
that = temp / np.linalg.norm(temp)
vhat = vel / vmag
####Solve the range doppler eqns iteratively
####Initial guess
zsch = height
for ii in range(10):
###Near nadir tests
if (hgt-zsch) >= rng:
return None
a = satDist
b = (radius + zsch)
costheta = 0.5*(a/rng + rng/a - (b/a)*(b/rng))
sintheta = np.sqrt(1-costheta*costheta)
gamma = rng*costheta
alpha = dopfact - gamma*np.dot(nhat,vhat)/np.dot(vhat,that)
beta = -side*np.sqrt(rng*rng*sintheta*sintheta - alpha*alpha)
delta = alpha * that + beta * chat + gamma * nhat
targVec = pos + delta
targLLH = refElp.xyz_to_llh(list(targVec))
targXYZ = np.array(refElp.llh_to_xyz([targLLH[0], targLLH[1], height]))
zsch = np.linalg.norm(targXYZ) - radius
rdiff = rng - np.linalg.norm(pos - targXYZ)
return targLLH
def rdr2geo(self, aztime, rng, height=0.,
doppler = None, wvl = None,
planet=None, side=-1):
'''
Returns point on ground at given height and doppler frequency.
Never to be used for heavy duty computing.
'''
from isceobj.Planet.Planet import Planet
####Setup doppler for the equations
dopfact = 0.0
hdg = self.getENUHeading(time=aztime)
sv = self.interpolateOrbit(aztime, method='hermite')
pos = sv.getPosition()
vel = sv.getVelocity()
vmag = np.linalg.norm(vel)
if doppler is not None:
dopfact = doppler(DTU.seconds_since_midnight(aztime), rng) * 0.5 * wvl * rng/vmag
if planet is None:
refElp = Planet(pname='Earth').ellipsoid
else:
refElp = planet.ellipsoid
###Convert position and velocity to local tangent plane
satLLH = refElp.xyz_to_llh(pos)
refElp.setSCH(satLLH[0], satLLH[1], hdg)
radius = refElp.pegRadCur
#####Setup ortho normal system right below satellite
satVec = np.array(pos)
velVec = np.array(vel)
###Setup TCN basis
clat = np.cos(np.radians(satLLH[0]))
slat = np.sin(np.radians(satLLH[0]))
clon = np.cos(np.radians(satLLH[1]))
slon = np.sin(np.radians(satLLH[1]))
nhat = np.array([-clat*clon, -clat*slon, -slat])
temp = np.cross(nhat, velVec)
chat = temp / np.linalg.norm(temp)
temp = np.cross(chat, nhat)
that = temp / np.linalg.norm(temp)
vhat = velVec / np.linalg.norm(velVec)
####Solve the range doppler eqns iteratively
####Initial guess
zsch = height
for ii in range(10):
###Near nadir tests
if (satLLH[2]-zsch) >= rng:
return None
a = (satLLH[2] + radius)
b = (radius + zsch)
costheta = 0.5*(a/rng + rng/a - (b/a)*(b/rng))
sintheta = np.sqrt(1-costheta*costheta)
gamma = rng*costheta
alpha = dopfact - gamma*np.dot(nhat,vhat)/np.dot(vhat,that)
beta = -side*np.sqrt(rng*rng*sintheta*sintheta - alpha*alpha)
delta = alpha * that + beta * chat + gamma * nhat
targVec = satVec + delta
targLLH = refElp.xyz_to_llh(list(targVec))
targXYZ = refElp.llh_to_xyz([targLLH[0], targLLH[1], height])
targSCH = refElp.xyz_to_sch(targXYZ)
zsch = targSCH[2]
rdiff = rng - np.linalg.norm(np.array(satVec) - np.array(targXYZ))
return targLLH
