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