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)