def testSCH2(self):
elp = Planet("Earth").get_elp()
#Peg at North Pole, S path on prime meridian heading North to South
elp.setSCH(90., 0., -90.)
#SCH = [0.,0.,0.] => XYZ = [elp.b, 0., 0.]
sch = [0., 0., 0.]
xyz = elp.sch_to_xyz(sch)
self.assertAlmostEqual(xyz[0], 0., places=3)
self.assertAlmostEqual(xyz[1], 0., places=3)
self.assertAlmostEqual(xyz[2], elp.b, places=3)
sch1 = elp.xyz_to_sch(xyz)
for (s, s1) in zip(sch, sch1):
self.assertAlmostEqual(s, s1, places=3)
#SCH = [pi*elp.pegRadCur, 0, elp.b+(elp.pegOV[2]-elp.pegRadCur)] =>
#XYZ=[0., 0., -elp.b]
sch = [
numpy.pi * elp.pegRadCur, 0., elp.b + elp.pegOV[2] - elp.pegRadCur
]
xyz = elp.sch_to_xyz(sch)
self.assertAlmostEqual(xyz[0], 0., places=3)
self.assertAlmostEqual(xyz[1], 0., places=3)
self.assertAlmostEqual(xyz[2], -elp.b, places=3)
sch1 = elp.xyz_to_sch(xyz)
for (s, s1) in zip(sch, sch1):
self.assertAlmostEqual(s, s1, places=3)
def testXYZSCH(self):
elp = Planet("Earth").get_elp()
elp.setSCH(30., 60., 45.)
sch = [-50000., 200000., 1000.]
xyz = elp.sch_to_xyz(sch)
sch1 = elp.xyz_to_sch(xyz)
for (s, s1) in zip(sch, sch1):
self.assertAlmostEqual(s, s1, places=3)
xyz = [-4.e6, 10.e6, 1.e6]
sch = elp.xyz_to_sch(xyz)
xyz1 = elp.sch_to_xyz(sch)
for (x, x1) in zip(xyz, xyz1):
self.assertAlmostEqual(x, x1, places=3)
elp.setSCH(65., -22., -30.)
sch = [100000., -100000., 100000.]
xyz = elp.sch_to_xyz(sch)
sch1 = elp.xyz_to_sch(xyz)
for (s, s1) in zip(sch, sch1):
self.assertAlmostEqual(s, s1, places=3)
xyz = [-1.e6, -2.e6, 100.e6]
sch = elp.xyz_to_sch(xyz)
xyz1 = elp.sch_to_xyz(sch)
for (x, x1) in zip(xyz, xyz1):
self.assertAlmostEqual(x, x1, places=3)
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
def testSCH1(self):
elp = Planet("Earth").get_elp()
#S path on Earth equator West to East, origin at y=z=0
elp.setSCH(0., 0., 90.)
#SCH = [0.,0.,0.] => XYZ = [elp.a, 0., 0.]
sch = [0., 0., 0.]
xyz = elp.sch_to_xyz(sch)
self.assertAlmostEqual(xyz[0], elp.a, places=3)
self.assertAlmostEqual(xyz[1], 0., places=3)
self.assertAlmostEqual(xyz[2], 0., places=3)
sch1 = elp.xyz_to_sch(xyz)
for (s, s1) in zip(sch, sch1):
self.assertAlmostEqual(s, s1, places=3)
#SCH = [(pi/2)*elp.a, 0, 0] => XYZ=[0., elp.a, 0.]
sch = [numpy.pi * elp.a / 2., 0., 0.]
xyz = elp.sch_to_xyz(sch)
self.assertAlmostEqual(xyz[0], 0., places=3)
self.assertAlmostEqual(xyz[1], elp.a, places=3)
self.assertAlmostEqual(xyz[2], 0., places=3)
sch1 = elp.xyz_to_sch(xyz)
for (s, s1) in zip(sch, sch1):
self.assertAlmostEqual(s, s1, places=3)
#SCH = [pi*elp.a, 0, 0] => XYZ=[-elp.a, 0., 0.]
sch = [numpy.pi * elp.a, 0., 0.]
xyz = elp.sch_to_xyz(sch)
self.assertAlmostEqual(xyz[0], -elp.a, places=3)
self.assertAlmostEqual(xyz[1], 0., places=3)
self.assertAlmostEqual(xyz[2], 0., places=3)
# Round off causes degenerate case where lon = -180 and lon=180 are the same
# point and xyz(-sch) = xyz(+sch), but -sch != sch
#
# sch1 = elp.xyz_to_sch(xyz)
# print(sch1)
# for (s,s1) in zip(sch, sch1):
# self.assertAlmostEqual(s, s1, places=3)
#SCH = [(3pi/2)*elp.a, 0, 0] => XYZ=[0., -elp.a, 0.]
sch = [3 * numpy.pi * elp.a / 2., 0., 0.]
xyz = elp.sch_to_xyz(sch)
self.assertAlmostEqual(xyz[0], 0., places=3)
self.assertAlmostEqual(xyz[1], -elp.a, places=3)
self.assertAlmostEqual(xyz[2], 0., places=3)
# sch1 = elp.xyz_to_sch(xyz)
# for (s,s1) in zip(sch, sch1):
# self.assertAlmostEqual(s, s1, places=3)
#SCH = [2pi*elp.a, 0, 0] => XYZ=[elp.a, 0., 0.]
sch = [2. * numpy.pi * elp.a, 0., 0.]
xyz = elp.sch_to_xyz(sch)
self.assertAlmostEqual(xyz[0], elp.a, places=3)
self.assertAlmostEqual(xyz[1], 0., places=3)
self.assertAlmostEqual(xyz[2], 0., places=3)
#Another sch degeneracy due to angle branch cut
# sch1 = elp.xyz_to_sch(xyz)
# for (s,s1) in zip(sch, sch1):
# self.assertAlmostEqual(s, s1, places=3)
#SCH = [0., (pi/2)*elp.a, elp.b-elp.a] => XYZ = [0., 0., elp.b]
sch = [0., numpy.pi * elp.a / 2., elp.b - elp.a]
xyz = elp.sch_to_xyz(sch)
self.assertAlmostEqual(xyz[0], 0., places=3)
self.assertAlmostEqual(xyz[1], 0., places=3)
self.assertAlmostEqual(xyz[2], elp.b, places=3)
# sch1 = elp.xyz_to_sch(xyz)
# for (s,s1) in zip(sch, sch1):
# self.assertAlmostEqual(s, s1, places=3)
#SCH = [0., pi*elp.a, 0.] => XYZ = [-elp.a, 0., 0.]
sch = [0., numpy.pi * elp.a, 0.]
xyz = elp.sch_to_xyz(sch)
self.assertAlmostEqual(xyz[0], -elp.a, places=3)
self.assertAlmostEqual(xyz[1], 0., places=3)
self.assertAlmostEqual(xyz[2], 0., places=3)
#SCH = [0., (3pi/2)*elp.a, elp.b-elp.a] => XYZ = [0., 0., -elp.b]
sch = [0., 3. * numpy.pi * elp.a / 2., elp.b - elp.a]
xyz = elp.sch_to_xyz(sch)
self.assertAlmostEqual(xyz[0], 0., places=3)
self.assertAlmostEqual(xyz[1], 0., places=3)
self.assertAlmostEqual(xyz[2], -elp.b, places=3)
