def test_aer_ecef(): xyz = pm.aer2ecef(*aer0, *lla0) assert xyz == approx(axyz0) assert pm.aer2ecef(*raer0, *rlla0, deg=False) == approx(axyz0) with pytest.raises(ValueError): pm.aer2ecef(aer0[0], aer0[1], -1, *lla0) assert pm.ecef2aer(*xyz, *lla0) == approx(aer0) assert pm.ecef2aer(*xyz, *rlla0, deg=False) == approx(raer0)
def mergefov(w0: xarray.Dataset, w1: xarray.Dataset, projalt: float = 110e3, method: str = None): """ inputs: ------- w0: wide FOV data, particularly az/el w1: other camera FOV data contained in w0 projalt: projection altitude METERS ofn: plot filename fovrow: to vastly speedup intiial overlap exploration, pick row(s) of pixels to examing--it could take half an hour + otherwise. find the ECEF x,y,z, at 110km altitude for narrow camera outer pixel boundary, then find the closest pixels in the wide FOV to those points. Remember, it can be (much) faster to brute force this calculation than to use k-d tree. """ if projalt < 1e3: logging.warning(f"this function expects meters, you picked projection altitude {projalt/1e3} km") # %% print distance from wide camera to narrow camera (just for information) print(f"intercamera distance with {w0.site}: {vdist(w0.lat,w0.lon, w1.lat,w1.lon)[0]/1e3:.1f} kilometers") # %% ENU projection from cam0 to cam1 e1, n1, u1 = pm.geodetic2enu(w1.lat, w1.lon, w1.alt_m, w0.lat, w0.lon, w0.alt_m) # %% find the ENU of narrow FOV pixels at 110km from narrow FOV w1 = pixelmask(w1, method) if method is not None and method.lower() == "mzslice": w0 = pixelmask(w0, method) azSlice0, elSlice0, rSlice0 = pm.ecef2aer(w1.x2mz, w1.y2mz, w1.z2mz, w0.lat, w0.lon, w0.alt_m) azSlice1, elSlice1, rSlice1 = pm.ecef2aer(w0.x2mz, w0.y2mz, w0.z2mz, w1.lat, w1.lon, w1.alt_m) # find image indices (mask) corresponding to slice az,el w0["rows"], w0["cols"] = fnd.findClosestAzel( w0["az"].where(w0["fovmask"]), w0["el"].where(w0["fovmask"]), azSlice0, elSlice0 ) w0.attrs["Brow"], w0.attrs["Bcol"] = fnd.findClosestAzel(w0["az"], w0["el"], w0.Baz, w0.Bel) w1["rows"], w1["cols"] = fnd.findClosestAzel( w1["az"].where(w1["fovmask"]), w1["el"].where(w1["fovmask"]), azSlice1, elSlice1 ) w1.attrs["Brow"], w1.attrs["Bcol"] = fnd.findClosestAzel(w1["az"], w1["el"], w1.Baz, w1.Bel) else: # csc(x) = 1/sin(x) slantrange = projalt / np.sin(np.radians(np.ma.masked_invalid(w1["el"].where(w1["fovmask"])))) assert (slantrange >= projalt).all(), "slantrange must be >= projection altitude" e0, n0, u0 = pm.aer2enu(w1["az"], w1["el"], slantrange) # %% find az,el to narrow FOV from ASI FOV az0, el0, _ = pm.enu2aer(e0 - e1, n0 - n1, u0 - u1) assert (el0 >= 0).all(), "FOVs may not overlap, negative elevation from cam0 to cam1" # %% nearest neighbor brute force w0["rows"], w0["cols"] = fnd.findClosestAzel(w0["az"], w0["el"], az0, el0) return w0, w1
def projectisrhist(isrlla,beamazel,optlla,optazel,heightkm): """ intended to project ISR beam at a single height into optical data. output: az,el,slantrange in degrees,meters """ isrlla = asarray(isrlla); optlla=asarray(optlla) assert isrlla.size == optlla.size == 3 x,y,z = aer2ecef(beamazel[0],beamazel[1],heightkm*1e3,isrlla[0],isrlla[1],isrlla[2]) try: az,el,srng = ecef2aer(x,y,z,optlla[0],optlla[1],optlla[2]) except IndexError: az,el,srng = ecef2aer(x,y,z,optlla['lat'],optlla['lon'],optlla['alt_km']) return {'az':az,'el':el,'srng':srng}
def gpsSatPositionSP3(fsp3, dt, sv=None, rx_position=None, coords='xyz'): assert sv is not None # Read in data D = gr.load(fsp3).sel(sv=sv) if isinstance(dt, list): dt = np.asarray(dt) dt = dt.astype('datetime64[s]') navtimes = D.time.values.astype('datetime64[s]') CSx = interpolate.CubicSpline(navtimes.astype(int), D.position.values[:, 0]) CSy = interpolate.CubicSpline(navtimes.astype(int), D.position.values[:, 1]) CSz = interpolate.CubicSpline(navtimes.astype(int), D.position.values[:, 2]) ecefxi = CSx(dt.astype(int)) * 1e3 ecefyi = CSy(dt.astype(int)) * 1e3 ecefzi = CSz(dt.astype(int)) * 1e3 if coords == 'xyz': return np.array([ecefxi, ecefyi, ecefzi]) else: AER = ecef2aer(x=ecefxi, y=ecefyi, z=ecefzi, lon0=rx_position[1], lat0=rx_position[0], h0=rx_position[2]) return np.array(AER)
def gloSatPosition(navfn, sv, obstimes, rx_position=None, cs='xyz'): obstimes = obstimes.astype('datetime64[s]') D = gr.load(navfn).sel(sv = sv) navtimes = D.time.values.astype('datetime64[s]') x = D.X.values * 1e3 y = D.Y.values * 1e3 z = D.Z.values * 1e3 # Rearange # deltaTin = np.diff(obstimes)[0] tmin = min(obstimes.min(), navtimes.min()) tmax = max(obstimes.max(), navtimes.max()) navtimes_interp = np.arange(tmin, tmax + 1, timedelta(seconds = 1)).astype('datetime64[s]') #deltaTin.item().total_seconds())).astype('datetime64[s]') x0 = np.linspace(0, 1, navtimes.shape[0]) x1 = np.linspace(0, 1, navtimes_interp.shape[0]) Xi = np.interp(x1, x0, x) Yi = np.interp(x1, x0, y) Zi = np.interp(x1, x0, z) # if isinstance(obstimes, (np.ndarray)): idt = np.isin(navtimes_interp, obstimes)#(navtimes_interp >= obstimes[0]) & (navtimes_interp <= obstimes[-1]) if cs == 'xyz': xyz = np.array([Xi[idt], Yi[idt], Zi[idt]]) return xyz elif cs == 'aer': assert rx_position is not None if isinstance(rx_position, list): rx_position = np.array(rx_position) assert rx_position.shape[0] == 3 aer = ecef2aer(Xi[idt],Yi[idt],Zi[idt], lon0 = rx_position[0], lat0 = rx_position[1], h0 = rx_position[2]) return aer else: return
def gpsSatPosition(fnav, dt, sv=None, rx_position=None, coords='xyz'): navdata = gr.load(fnav).sel(sv=sv) timesarray = np.asarray(dt,dtype='datetime64[ns]') #[datetime64 [ns]] # Manipulate with times, epochs and crap like this navtimes = navdata.time.values # [datetime64 [ns]] idnan = np.isfinite(navdata['Toe'].values) navtimes = navtimes[idnan] bestephind = [] for t in timesarray: idt = abs(navtimes - t).argmin() #if t>navtimes[abs(navtimes - t).argmin()] else abs(navtimes - t).argmin()-1 bestephind.append(idt) # bestephind = np.array([np.argmin(abs(navtimes-t)) for t in timesarray]) gpstime = np.array([getGpsTime(t) for t in dt]) t = gpstime - navdata['Toe'][idnan][bestephind].values # [datetime.datetime] # constants GM = 3986005.0E8 # universal gravational constant OeDOT = 7.2921151467E-5 # Elements ecc = navdata['Eccentricity'][idnan][bestephind].values # Eccentricity Mk = navdata['M0'][idnan][bestephind].values + \ t *(np.sqrt(GM / navdata['sqrtA'][idnan][bestephind].values**6) + navdata['DeltaN'][idnan][bestephind].values) Ek = solveIter(Mk,ecc) Vk = np.arctan2(np.sqrt(1.0 - ecc**2) * np.sin(Ek), np.cos(Ek) - ecc) PhiK = Vk + navdata['omega'][idnan][bestephind].values # Perturbations delta_uk = navdata['Cuc'][idnan][bestephind].values * np.cos(2.0*PhiK) + \ navdata['Cus'][idnan][bestephind].values * np.sin(2.0*PhiK) Uk = PhiK + delta_uk delta_rk = navdata['Crc'][idnan][bestephind].values * np.cos(2.0*PhiK) + \ navdata['Crs'][idnan][bestephind].values * np.sin(2.0*PhiK) Rk = navdata['sqrtA'][idnan][bestephind].values**2 * (1.0 - ecc * np.cos(Ek)) + delta_rk delta_ik = navdata['Cic'][idnan][bestephind].values * np.cos(2.0*PhiK) + \ navdata['Cis'][idnan][bestephind].values * np.sin(2.0*PhiK) Ik = navdata['Io'][idnan][bestephind].values + \ navdata['IDOT'][idnan][bestephind].values * t + delta_ik #Compute the right ascension Omegak = navdata['Omega0'][idnan][bestephind].values + \ (navdata['OmegaDot'][idnan][bestephind].values - OeDOT) * t - \ (OeDOT * navdata['Toe'][idnan][bestephind].values) #X,Y coordinate corrections Xkprime = Rk * np.cos(Uk) Ykprime = Rk * np.sin(Uk) # ECEF XYZ X = Xkprime * np.cos(Omegak) - (Ykprime * np.sin(Omegak) * np.cos(Ik)) Y = Xkprime * np.sin(Omegak) + (Ykprime * np.cos(Omegak) * np.cos(Ik)) Z = Ykprime * np.sin(Ik) if coords == 'xyz': return np.array([X,Y,Z]) elif coords == 'aer': assert rx_position is not None rec_lat, rec_lon, rec_alt = ecef2geodetic(rx_position[0], rx_position[1], rx_position[2]) A,E,R = ecef2aer(X, Y, Z, rec_lat, rec_lon, rec_alt) return np.array([A,E,R])
def projectisrhist(isrlla, beamazel, optlla, optazel, heightkm): """ intended to project ISR beam at a single height into optical data. output: az,el,slantrange in degrees,meters """ isrlla = asarray(isrlla) optlla = asarray(optlla) assert isrlla.size == optlla.size == 3 x, y, z = aer2ecef(beamazel[0], beamazel[1], heightkm * 1e3, isrlla[0], isrlla[1], isrlla[2]) try: az, el, srng = ecef2aer(x, y, z, optlla[0], optlla[1], optlla[2]) except IndexError: az, el, srng = ecef2aer(x, y, z, optlla["lat"], optlla["lon"], optlla["alt_km"]) return {"az": az, "el": el, "srng": srng}
def getIonosphericPiercingPoints(rx_xyz, sv, obstimes, ipp_alt, navfn, cs='wsg84', rx_xyz_coords='xyz', el0=0): """ Sebastijan Mrak Function returns a list of Ionospheric Piersing Point (IPP) trajectory in WSG84 coordinate system (CS). Function takes as parameter a receiver location in ECEF CS, satellite number, times ob observation and desired altitude of a IPP trajectory. You also have to specify a full path to th navigation data file. It returns IPP location in either WSG84 or AER coordinate system. """ ipp_alt = ipp_alt * 1E3 if rx_xyz_coords == 'xyz': rec_lat, rec_lon, rec_alt = ecef2geodetic(rx_xyz[0], rx_xyz[1], rx_xyz[2]) else: if not isinstance(rx_xyz, list): rx_xyz = list(rx_xyz) if len(rx_xyz) == 2: rx_xyz.append(0) assert len(rx_xyz) == 3 rec_lat = rx_xyz[0] rec_lon = rx_xyz[1] rec_alt = rx_xyz[2] rx_xyz = geodetic2ecef(lat = rec_lon, lon = rec_lat, alt = rec_alt) if sv[0] == 'G': if navfn.endswith('n'): xyz = gpsSatPosition(navfn, obstimes, sv=sv, rx_position=rx_xyz, coords='xyz') elif navfn.endswith('sp3'): xyz = gpsSatPositionSP3(navfn, obstimes, sv=sv, rx_position=rx_xyz, coords='xyz') az,el,r = ecef2aer(xyz[0,:],xyz[1,:],xyz[2,:],rec_lat, rec_lon, rec_alt) aer_vector = np.array([az, el, r]) r_new = [] for i in range(len(el)): if el[i] > el0: fm = np.sin(np.radians(el[i])) r_new.append(ipp_alt / fm) else: r_new.append(np.nan) lla_vector = np.array(aer2geodetic(az, el, r_new, rec_lat, rec_lon, rec_alt)) elif sv[0] == 'R': aer_vector = gloSatPosition(navfn=navfn, sv=sv, obstimes=obstimes, rx_position=[rec_lon, rec_lat, rec_alt], cs='aer') fm = np.sin(np.radians(aer_vector[1])) r_new = ipp_alt / fm lla_vector = np.array(aer2geodetic(aer_vector[0], aer_vector[1], r_new, rec_lat, rec_lon, rec_alt)) else: print ('Type in valid sattype initial. "G" for GPS and "R" for GLONASS') if (cs == 'wsg84'): return lla_vector elif (cs == 'aer'): return aer_vector else: print ('Enter either "wsg84" or "aer" as coordinate system. "wsg84" is default one.')
def get1Dcut(cam,odir,verbose): """ i. get az/el of each pixel (rotated/transposed as appropriate) ii. get cartesian ECEF of each pixel end, a point outside the grid (to create rays to check intersections with grid) iii. put cameras in same frame, getting az/el to each other's pixel ends iv. find the indices corresponding to those angles now the cameras are geographically registered to pixel indices """ #%% determine slant range between other camera and magnetic zenith to evaluate at srpts = logspace(4.3,6.9,25) #4.5 had zero discards for hst0 #6.8 didn't quite get to zenith #%% (i) load az/el data from Astrometry.net for C in cam: if C.usecam: C.doorient() C.toecef(srpts) #optional: plot ECEF of points between each camera and magnetic zenith (lying at az,el relative to each camera) if verbose: plotLOSecef(cam,odir) #%% (2) get az,el of these points from camera to the other camera's points cam[0].az2pts,cam[0].el2pts,cam[0].r2pts = ecef2aer(cam[1].x2mz, cam[1].y2mz, cam[1].z2mz, cam[0].lat, cam[0].lon, cam[0].alt_m) cam[1].az2pts,cam[1].el2pts,cam[1].r2pts = ecef2aer(cam[0].x2mz, cam[0].y2mz, cam[0].z2mz, cam[1].lat, cam[1].lon, cam[1].alt_m) #%% (3) find indices corresponding to these az,el in each image # and Least squares fit line to nearest points found in step 3 for C in cam: if C.usecam: C.findClosestAzel(odir) #%% if verbose and odir: dbgfn = odir / 'debugLSQ.h5' print('writing', dbgfn) with h5py.File(str(dbgfn),'w',libver='latest') as f: for C in cam: f[f'/cam{C.name}/cutrow'] = C.cutrow f[f'/cam{C.name}/cutcol'] = C.cutcol f['/cam{C.name}/xpix'] = C.xpix return cam
def getPP(satpos, sv, recpos, pph, err=1.0): """ get az and el to the satellite and repeatedly increase the range, converting to LLA each time to check the altitude. Stop when all the altitudes are within err of pph. Inputs satellite position array in ECEF, satellite number, receiver position in ECEF, pierce point height in km and error in km if you want. """ rlat, rlon, ralt = ecef2geodetic(recpos) sataz, satel, satr = ecef2aer(satpos[:, 0], satpos[:, 1], satpos[:, 2], rlat, rlon, ralt) r = np.zeros(len(satr)) pplat, pplon, ppalt = aer2geodetic(sataz, satel, r, rlat, rlon, ralt) mask = (ppalt / 1000 - pph) < 0 while np.sum(mask) > 0: r[mask] += 100 pplat, pplon, ppalt = aer2geodetic(sataz, satel, r * 1000, rlat, rlon, ralt) mask = (ppalt / 1000 - pph) < 0 sRange = r - 100.0 eRange = r count = 0 while not np.all(abs(ppalt / 1000 - pph) < err): count += 1 mRange = (sRange + eRange) / 2.0 pplat, pplon, ppalt = aer2geodetic(sataz, satel, mRange * 1000, rlat, rlon, ralt) mask = ppalt / 1000 > pph eRange[mask] = mRange[mask] sRange[~mask] = mRange[~mask] if (count > 100): raise TypeError('going too long') break ppalt = pph * 1000 return pplat, pplon, ppalt
def gpsSatPositionSP3(fnav, dt, sv=None, rx_position=None, coords='xyz'): assert sv is not None # Read in data D = grx.load(fnav).sel(sv=sv) dt = dt.astype('datetime64[s]') navtimes = D.time.values.astype('datetime64[s]') CSx = CubicSpline(navtimes.astype(int), D.ecef.values[:, 0]) CSy = CubicSpline(navtimes.astype(int), D.ecef.values[:, 1]) CSz = CubicSpline(navtimes.astype(int), D.ecef.values[:, 2]) ecefxi = CSx(obstimes.astype(int)) ecefyi = CSy(obstimes.astype(int)) ecefzi = CSz(obstimes.astype(int)) if coords == 'xyz': return np.array([ecefxi, ecefyi, ecefzi]) else: AER = ecef2aer(x=ecefxi, y=ecefyi, z=ecefzi, lon0=rx_position[1], lat0=rx_position[0], h0=rx_position[2]) return AER
def main(): path = "LS2D.txt" start = input("Enter start date and time (UTC, blank=system time) " \ + "(YYYY MM DD HH MM SS MICROS): (OPTIONAL!)") if start.strip() == "": time = datetime.datetime.utcnow() print(" \033[36mUsing current system time (UTC): " \ + time.isoformat(' ') + "\033[0m") else: try: time = datetime.datetime.strptime(start.strip(), "%Y %m %d %H %M %S %f") print(" \033[36mParsed start time as: " + time.isoformat(' ') \ + "\033[0m") except: print(" \033[31mUnable to parse start time from: " + start) print(" example of suitable input: " \ + "2019 01 09 22 05 16 01\033[0m") exit(1) inc_field = input("Enter field to be incremented [hr, min, sec, us]: ") try: inc = int(input("Enter incrementation value: ")) except: print(" \033[31mUnable to parse value\033[0m") exit(1) us_inc = inc if inc_field.strip() == "hr": us_inc = us_inc * 3.6e9 print(" \033[36mEach time step will increase by " + str(inc) \ + " hour(s) (" + str(us_inc) + "μs)\033[0m") elif inc_field.strip() == "min": us_inc = us_inc * 6e7 print(" \033[36mEach time step will increase by " + str(inc) \ + " minute(s) (" + str(us_inc) + "μs)\033[0m") elif inc_field.strip() == "sec": us_inc = us_inc * 1e6 print(" \033[36mEach time step will increase by " + str(inc) \ + " second(s) (" + str(us_inc) + "μs)\033[0m") elif inc_field.strip() == "us": print(" \033[36mEach time step will increase by " + str(us_inc) \ + "μs\033[0m") else: print(" \033[31mUnable to parse time increment field\033[0m") exit(1) try: num_samples = int(\ input("Enter total number of samples: ")) except: print(" \033[31mUnable to parse value\033[0m") exit(1) f = open(path, "r") for x in f: if x.startswith("1 "): line1 = "$$" + x + "$" print("Line1: %s" % (line1)) elif x.startswith("2 "): line2 = ".." + x + "." print("Line2: %s" % (line2)) else: output_filename = x[:-5] + time.strftime("%Y_%m_%d_%H_%M_%S_%f") + \ "-" + str(us_inc) + "-" + str(num_samples) + ".csv" print("File name: " + output_filename) f.close() satellite = twoline2rv(line1, line2, wgs84) outfile = open(output_filename, "w") for i in range(num_samples): datestamp = time.strftime("%Y,%m,%d,%H,%M,%S.%f") second = float(str(time.second) + "." + str(time.microsecond)) position, v = satellite.propagate(time.year, time.month, time.day, \ time.hour, time.minute, second) position_string = "," if (position[0] > 0): position_string += " " position_string += str(position[0]) + "," while len(position_string) < 16: position_string += " " if (position[1] > 0): position_string += " " position_string += str(position[1]) + "," while len(position_string) < 31: position_string += " " if (position[2] > 0): position_string += " " position_string += str(position[2]) #print("x, y, z" +": " + str(position[0]) +", "+ str(position[1]) +", "+ str(position[2])) x = position[0] y = position[1] z = position[2] ## Fabien check this!!!! using ecef2aer function lat0, long0, h0 = 43.58, 7.12, 10 az, el, srange = pm.ecef2aer(position[0] * 1000, position[1] * 1000, position[2] * 1000, lat0, long0, h0, None, True) print("az, el, srange, dist_to_earth_center: " + \ str(az) +", " + str(el) + ", " + str(srange)+ ", " + str(abs((x**2 +y**2 + z**2))**(1/2))) outfile.write(datestamp + position_string + ", " +\ str(az) +", " + str(el) + ", " + str(srange)+ ", " + str(abs((x**2 +y**2 + z**2))**(1/2))+ "\n") time = time + datetime.timedelta(microseconds=us_inc) outfile.close()
def test_geodetic(self): if pyproj: ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84') lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84') x1, y1, z1 = pm.geodetic2ecef(tlat, tlon, talt) assert_allclose( pm.geodetic2ecef(radians(tlat), radians(tlon), talt, deg=False), (x1, y1, z1)) assert_allclose((x1, y1, z1), (x0, y0, z0), err_msg='geodetic2ecef') assert_allclose(pm.ecef2geodetic(x1, y1, z1), (tlat, tlon, talt), err_msg='ecef2geodetic') if pyproj: assert_allclose(pyproj.transform(lla, ecef, tlon, tlat, talt), (x1, y1, z1)) assert_allclose(pyproj.transform(ecef, lla, x1, y1, z1), (tlon, tlat, talt)) lat2, lon2, alt2 = pm.aer2geodetic(taz, tel, tsrange, tlat, tlon, talt) assert_allclose((lat2, lon2, alt2), (lat1, lon1, alt1), err_msg='aer2geodetic') assert_allclose(pm.geodetic2aer(lat2, lon2, alt2, tlat, tlon, talt), (taz, tel, tsrange), err_msg='geodetic2aer') x2, y2, z2 = pm.aer2ecef(taz, tel, tsrange, tlat, tlon, talt) assert_allclose( pm.aer2ecef(radians(taz), radians(tel), tsrange, radians(tlat), radians(tlon), talt, deg=False), (a2x, a2y, a2z)) assert_allclose((x2, y2, z2), (a2x, a2y, a2z), err_msg='aer2ecef') assert_allclose(pm.ecef2aer(x2, y2, z2, tlat, tlon, talt), (taz, tel, tsrange), err_msg='ecef2aer') e1, n1, u1 = pm.aer2enu(taz, tel, tsrange) assert_allclose((e1, n1, u1), (e0, n0, u0), err_msg='aer2enu') assert_allclose(pm.aer2ned(taz, tel, tsrange), (n0, e0, -u0), err_msg='aer2ned') assert_allclose(pm.enu2aer(e1, n1, u1), (taz, tel, tsrange), err_msg='enu2aer') assert_allclose(pm.ned2aer(n1, e1, -u1), (taz, tel, tsrange), err_msg='ned2aer') assert_allclose(pm.enu2ecef(e1, n1, u1, tlat, tlon, talt), (x2, y2, z2), err_msg='enu2ecef') assert_allclose(pm.ecef2enu(x2, y2, z2, tlat, tlon, talt), (e1, n1, u1), err_msg='ecef2enu') assert_allclose(pm.ecef2ned(x2, y2, z2, tlat, tlon, talt), (n1, e1, -u1), err_msg='ecef2ned') assert_allclose(pm.ned2ecef(n1, e1, -u1, tlat, tlon, talt), (x2, y2, z2), err_msg='ned2ecef') # %% assert_allclose(pm.ecef2enuv(vx, vy, vz, tlat, tlon), (ve, vn, vu)) assert_allclose(pm.ecef2nedv(vx, vy, vz, tlat, tlon), (vn, ve, -vu)) #%% e3, n3, u3 = pm.geodetic2enu(lat2, lon2, alt2, tlat, tlon, talt) assert_allclose(pm.geodetic2ned(lat2, lon2, alt2, tlat, tlon, talt), (n3, e3, -u3)) assert_allclose(pm.enu2geodetic(e3, n3, u3, tlat, tlon, talt), (lat2, lon2, alt2), err_msg='enu2geodetic') assert_allclose(pm.ned2geodetic(n3, e3, -u3, tlat, tlon, talt), (lat2, lon2, alt2), err_msg='ned2geodetic')
fnav = 'brdc2440.17n' DOBS = pyGnss.dataFromNC(fnc, fnav, sv='G07', tlim=None, el_mask=30, satpos=True) obstimes = DOBS.time.values.astype('datetime64[s]') RX = DOBS.position_geodetic fsp3 = glob(os.getcwd() + '/*.sp3')[0] D = grx.load(fsp3).sel(sv='G07') navtimes = D.time.values.astype('datetime64[s]') AER = ecef2aer(x=D.ecef.values[:, 0], y=D.ecef.values[:, 1], z=D.ecef.values[:, 2], lon0=RX[1], lat0=RX[0], h0=RX[2]) ecef = pyGnss.gpsSatPositionSP3(fsp3, dt=obstimes, sv='G07', rx_position=RX, coords='xyz') AERi = pyGnss.gpsSatPositionSP3(fsp3, dt=obstimes, sv='G07', rx_position=RX, coords='aer') fig = plt.figure(figsize=[8, 6])
def map_target(tx, rx, az, el, rf): """ Find the scatter location given tx location, rx, location, total rf distance, and target angle-of-arrival. Parameters ---------- tx : float np.array [latitude, longitude, altitude] of tx array in degrees and kilometers rx : float np.array [latitude, longitude, altitude] of rx array in degrees and kilometers az : float angle-of-arrival azimuth in degrees el : float angle-of-arrival elevation in degrees rf : float np.array total rf path distance rf = c * tau Returns ------- sx : float np.array [latitude, longitude, altitude] of scatter in degrees and kilometers r : float bistatic slant range in kilometers """ # Setup givens in correct units rf = rf * 1.0e3 * 1.5 - 230e3 az = np.where(az < 0, np.deg2rad(az + 367.0), np.deg2rad(az + 7.0)) el = np.deg2rad(np.abs(el)) sx = np.zeros((3, len(rf))) uv = np.zeros((3, len(rf))) us = np.zeros((3, len(rf))) # Determine the slant range, r bx1, by1, bz1 = pm.geodetic2ecef(rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) ur = np.array([bx1, by1, bz1]) / np.linalg.norm([bx1, by1, bz1]) bx2, by2, bz2 = pm.geodetic2ecef(tx[0], tx[1], tx[2], ell=pm.Ellipsoid("wgs84"), deg=True) ut = np.array([bx2, by2, bz2]) / np.linalg.norm([bx2, by2, bz2]) bx = bx2 - bx1 by = by2 - by1 bz = bz2 - bz1 b = np.linalg.norm([bx, by, bz]) ub = np.array([bx, by, bz]) / b ua = np.array( [np.sin(az) * np.cos(el), np.cos(az) * np.cos(el), np.sin(el)]) theta = np.arccos(ua[0, :] * ub[0] + ua[1, :] * ub[1] + ua[2, :] * ub[2]) r = (rf**2 - b**2) / (2 * (rf - b * np.cos(theta))) # Correct elevation using geocentric angle gamma and first order ranges, find scatter lat, long, alt for i in range(len(rf)): bx3, by3, bz3 = pm.aer2ecef(np.rad2deg(az[i]), np.rad2deg(el[i]), np.abs(r[i]), rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) us[:, i] = np.array([bx3, by3, bz3]) / np.linalg.norm([bx3, by3, bz3]) #el[i] -= np.arccos(ut[0]*us[0, i] + ut[1]*us[1, i] + ut[2]*us[2, i]) el[i] -= np.arccos(ur[0] * us[0, i] + ur[1] * us[1, i] + ur[2] * us[2, i]) sx[:, i] = pm.aer2geodetic(np.rad2deg(az[i]), np.rad2deg(el[i]), np.abs(r[i]), rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) # Second order slant range, r ua = np.array( [np.sin(az) * np.cos(el), np.cos(az) * np.cos(el), np.sin(el)]) theta = np.arccos(ua[0, :] * ub[0] + ua[1, :] * ub[1] + ua[2, :] * ub[2]) r = (rf**2 - b**2) / (2 * (rf - b * np.cos(theta))) for i in range(len(rf)): sx[:, i] = pm.aer2geodetic(np.rad2deg(az[i]), np.rad2deg(el[i]), np.abs(r[i]), rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) # Find the bistatic bisector velocity unit vector uv = (us + ua) / 2.0 uv = uv / np.linalg.norm(uv) # Set units to degrees and kilometers sx[2, :] /= 1.0e3 r /= 1.0e3 # d = rf/2 - 200e3 # r1 = np.sqrt((6378.1370e3 * np.cos(np.deg2rad(52.1579))) ** 2 + (6356.7523e3 * np.sin(np.deg2rad(52.1579))) ** 2) # pre_alt = np.sqrt(r1 ** 2 + (d) ** 2 - 2 * r1 * (d) * np.cos(np.pi/2 + el)) # el -= np.arccos(((d) ** 2 - (r1 ** 2) - (pre_alt ** 2)) / (-2 * r1 * pre_alt)) # Find lat, long, alt of target # sx = np.zeros((3, len(rf))) # for i in range(len(rf)): # sx[:, i] = pm.aer2geodetic(np.rad2deg(az[i]), np.rad2deg(el[i]), np.abs(r[i]), # rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) #return sx, r, uv vaz, vel, _ = pm.ecef2aer(uv[0, :] + bx3, uv[1, :] + by3, uv[2, :] + bz3, sx[0, :], sx[1, :], sx[2, :], ell=pm.Ellipsoid("wgs84"), deg=True) return sx[2, :], r, vaz, vel
def test_ecef2aer(xyz, lla, aer): assert pm.ecef2aer(*xyz, *lla) == approx(aer)
def test_geodetic(self): if pyproj: ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84') lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84') xyz1 = pm.geodetic2ecef(*lla0) assert_allclose(pm.geodetic2ecef(*rlla0, deg=False), xyz1, err_msg='geodetic2ecef: rad') assert_allclose(xyz1, xyz0, err_msg='geodetic2ecef: deg') assert_allclose(pm.ecef2geodetic(*xyz1), lla0, err_msg='ecef2geodetic: deg') assert_allclose(pm.ecef2geodetic(*xyz1, deg=False), rlla0, err_msg='ecef2geodetic: rad') if pyproj: assert_allclose( pyproj.transform(lla, ecef, lla0[1], lla0[0], lla0[2]), xyz1) assert_allclose(pyproj.transform(ecef, lla, *xyz1), (lla0[1], lla0[0], lla0[2])) lla2 = pm.aer2geodetic(*aer0, *lla0) rlla2 = pm.aer2geodetic(*raer0, *rlla0, deg=False) assert_allclose(lla2, lla1, err_msg='aer2geodetic: deg') assert_allclose(rlla2, rlla1, err_msg='aer2geodetic:rad') assert_allclose(pm.geodetic2aer(*lla2, *lla0), aer0, err_msg='geodetic2aer: deg') assert_allclose(pm.geodetic2aer(*rlla2, *rlla0, deg=False), raer0, err_msg='geodetic2aer: rad') # %% aer-ecef xyz2 = pm.aer2ecef(*aer0, *lla0) assert_allclose(pm.aer2ecef(*raer0, *rlla0, deg=False), axyz0, err_msg='aer2ecef:rad') assert_allclose(xyz2, axyz0, err_msg='aer2ecef: deg') assert_allclose(pm.ecef2aer(*xyz2, *lla0), aer0, err_msg='ecef2aer:deg') assert_allclose(pm.ecef2aer(*xyz2, *rlla0, deg=False), raer0, err_msg='ecef2aer:rad') # %% aer-enu enu1 = pm.aer2enu(*aer0) ned1 = (enu1[1], enu1[0], -enu1[2]) assert_allclose(enu1, enu0, err_msg='aer2enu: deg') assert_allclose(pm.aer2enu(*raer0, deg=False), enu0, err_msg='aer2enu: rad') assert_allclose(pm.aer2ned(*aer0), ned0, err_msg='aer2ned') assert_allclose(pm.enu2aer(*enu1), aer0, err_msg='enu2aer: deg') assert_allclose(pm.enu2aer(*enu1, deg=False), raer0, err_msg='enu2aer: rad') assert_allclose(pm.ned2aer(*ned1), aer0, err_msg='ned2aer') # %% enu-ecef assert_allclose(pm.enu2ecef(*enu1, *lla0), xyz2, err_msg='enu2ecef: deg') assert_allclose(pm.enu2ecef(*enu1, *rlla0, deg=False), xyz2, err_msg='enu2ecef: rad') assert_allclose(pm.ecef2enu(*xyz2, *lla0), enu1, err_msg='ecef2enu:deg') assert_allclose(pm.ecef2enu(*xyz2, *rlla0, deg=False), enu1, err_msg='ecef2enu:rad') assert_allclose(pm.ecef2ned(*xyz2, *lla0), ned1, err_msg='ecef2ned') assert_allclose(pm.ned2ecef(*ned1, *lla0), xyz2, err_msg='ned2ecef') # %% assert_allclose(pm.ecef2enuv(vx, vy, vz, *lla0[:2]), (ve, vn, vu)) assert_allclose(pm.ecef2nedv(vx, vy, vz, *lla0[:2]), (vn, ve, -vu)) # %% enu3 = pm.geodetic2enu(*lla2, *lla0) ned3 = (enu3[1], enu3[0], -enu3[2]) assert_allclose(pm.geodetic2ned(*lla2, *lla0), ned3, err_msg='geodetic2ned: deg') assert_allclose(pm.enu2geodetic(*enu3, *lla0), lla2, err_msg='enu2geodetic') assert_allclose(pm.ned2geodetic(*ned3, *lla0), lla2, err_msg='ned2geodetic')
def GDfromRinex(rinexfile, navfile, satFile, C1BiasFile, h5file=None, writeh5=False, pph=350, satlist=None): """ this function goes through the entire process, parses data from rinex, calculates sTEC, vTEC, satellite position, pierce point, receiver bias, etc. and turns it all into a GeoData object for plotting and stuff """ head, data = rinexobs(rinexfile, returnHead=True, h5file=h5file, writeh5=writeh5) nav = readRinexNav(navfile) svBiasObj = satelliteBias(satFile, C1BiasFile, None) extra = np.nan * np.ones((14, data.shape[1], data.shape[2], data.shape[3])) recpos = np.asarray(head['APPROX POSITION XYZ'], float)[:, None] rlat, rlon, ralt = ecef2geodetic(recpos) print('sv', end=': ') for sv in data.items: print(sv, end=' ') if ((sv, 1) not in svBiasObj.dict): continue satbias = svBiasObj.dict[(sv, 1)] #get time intervals, points where there is good tec ranges = getIntervals(data, sv) teclist = [] timelist = [] errlist = [] rbeg = [] pos = 0 for drange in ranges: rbeg.append(pos) tec, err = getTec(data, sv, drange) tec -= satbias teclist.append(tec) timelist.append(tec.index) errlist.append(err * np.ones(len(tec))) pos += len(tec) if len(teclist) == 0 or len(timelist) == 0: continue stec = Series(np.hstack((p for p in teclist)), index=np.hstack((t for t in timelist))) ntec = Series(np.hstack((j for j in errlist)), index=np.hstack((t for t in timelist))) for i, p in enumerate(rbeg): rbeg[i] -= np.sum(np.isnan(stec[:p])) rbeg = np.array(rbeg) ntec = ntec[~np.isnan(stec)] stec = stec[~np.isnan(stec)] satpos = getSatXYZ(nav, sv, stec.index) az, el, r = ecef2aer(satpos[:, 0], satpos[:, 1], satpos[:, 2], rlat, rlon, ralt) satpossph = np.vstack([az, el, r / 1000]).T goodtimes = np.in1d(data.major_axis, stec.index) #times for satellite with data svi = list(data.items).index( sv) # matrix column corresponding to satellite extra[:3, svi, goodtimes, 0] = satpos.T #XYZ extra[3:6, svi, goodtimes, 0] = satpossph.T #Spherical extra[6, svi, goodtimes, 0] = stec.values #TEC z = getZ(satpossph[:, 1]) extra[7, svi, goodtimes, 0] = z #vertical mapping function pplat, pplon, ppalt = getPP(satpos, sv, recpos, pph) #Pierce Point extra[8, svi, goodtimes, 0] = pplat extra[9, svi, goodtimes, 0] = pplon extra[10, svi, goodtimes, 0] = ppalt extra[11, svi, goodtimes, 0] = ntec.values #err tec splittimes = np.where(goodtimes)[0][rbeg] extra[13, svi, splittimes, 0] = 1 data['X'] = extra[0] data['Y'] = extra[1] data['Z'] = extra[2] data['Az'] = extra[3] data['El'] = extra[4] data['R'] = extra[5] data['TEC'] = extra[6] data['zmap'] = extra[7] data['pplat'] = extra[8] data['pplon'] = extra[9] data['ppalt'] = extra[10] data['nTEC'] = extra[11] print() print('recbias', end=': ') recbias = minScalBias(data, recpos) #calculate receiver bias extra[6, :, :, 0] -= recbias extra[12, :, :, 0] = (extra[6, :, :, 0]) / extra[7, :, :, 0] #vtec print(recbias) data['TEC'] = extra[6] #TEC adjusted with receiver bias data['vTEC'] = extra[12] #vTEC adjusted with receiver bias data['cslip'] = extra[13] d = { 'TEC': [], 'az2sat': [], 'el2sat': [], 'recBias': [], 'satnum': [], 'vTEC': [], 'nTEC': [], 'lol': [], 'raw': [] } dataloc = [] times = [] if (satlist == None): satlist = data.items for sv in satlist: msk = np.isfinite(data['TEC', sv, :, 'data']) #mask of times with data phase = 2.85E9 * (data['L1', sv, :, 'data'] / f1 - data['L2', sv, :, 'data'] / f2) d['raw'].append(phase[msk]) lol = data[['L1', 'L2', 'C1', 'P2'], sv, msk, 'lli'] lol[np.isnan(lol)] = 0 lol = lol.astype(int) lol = np.logical_or.reduce((lol % 2).T) lol = lol.astype( int) #store all hardware-determined loss of lock as a 1 greg = np.isfinite( data['cslip', sv, msk, 'data'].values) #mask of software determined cycle slips lol[greg] += 2 #add 2 to all times with cycle slips, HW=1, SW=2, both=3 d['lol'].append(lol) d['TEC'].append(data['TEC', sv, :, 'data'][msk]) d['az2sat'].append(data['Az', sv, :, 'data'][msk]) d['el2sat'].append(data['El', sv, :, 'data'][msk]) d['recBias'].append(recbias * np.ones(len(data['TEC', sv, :, 'data'][msk]))) d['satnum'].append(sv * np.ones(len(data['TEC', sv, :, 'data'][msk]))) d['vTEC'].append(data['vTEC', sv, :, 'data'][msk]) d['nTEC'].append(data['nTEC', sv, :, 'data'][msk]) dataloc.append(data[['pplat', 'pplon', 'ppalt'], sv, :, 'data'][msk]) times.append( np.hstack((data.major_axis[msk][:, None], data.major_axis[msk][:, None] + 1000000000))) d['raw'] = np.hstack(d['raw']) d['lol'] = np.hstack(d['lol']) d['TEC'] = np.hstack(d['TEC']) d['az2sat'] = np.hstack(d['az2sat']) d['el2sat'] = np.hstack(d['el2sat']) d['recBias'] = np.hstack(d['recBias']) d['satnum'] = np.hstack(d['satnum']) d['vTEC'] = np.hstack(d['vTEC']) d['nTEC'] = np.hstack(d['nTEC']) coordnames = 'WGS84' dataloc = np.vstack(dataloc) sensorloc = np.nan * np.ones(3) times = np.vstack(times) t0 = np.datetime64(datetime(1970, 1, 1), 'ns') times = (times - t0).astype(float) / 1.0E9 return (d, coordnames, dataloc, sensorloc, times)
def test_ecefenu(): assert_allclose(pm.aer2ecef(taz, tel, tsrange, tlat, tlon, talt), (a2x, a2y, a2z), rtol=0.01, err_msg='aer2ecef: {}'.format( pm.aer2ecef(taz, tel, tsrange, tlat, tlon, talt))) assert_allclose(pm.aer2enu(taz, tel, tsrange), (a2e, a2n, a2u), rtol=0.01, err_msg='aer2enu: ' + str(pm.aer2enu(taz, tel, tsrange))) assert_allclose(pm.aer2ned(taz, tel, tsrange), (a2n, a2e, -a2u), rtol=0.01, err_msg='aer2ned: ' + str(pm.aer2ned(taz, tel, tsrange))) assert_allclose(pm.ecef2enu(tx, ty, tz, tlat, tlon, talt), (e2e, e2n, e2u), rtol=0.01, err_msg='ecef2enu: {}'.format( pm.ecef2enu(tx, ty, tz, tlat, tlon, talt))) assert_allclose(pm.ecef2enuv(vx, vy, vz, tlat, tlon), (ve, vn, vu)) assert_allclose(pm.ecef2ned(tx, ty, tz, tlat, tlon, talt), (e2n, e2e, -e2u), rtol=0.01, err_msg='ecef2ned: {}'.format( pm.ecef2enu(tx, ty, tz, tlat, tlon, talt))) assert_allclose(pm.ecef2nedv(vx, vy, vz, tlat, tlon), (vn, ve, -vu)) assert_allclose(pm.aer2geodetic(taz, tel, tsrange, tlat, tlon, talt), (a2la, a2lo, a2a), rtol=0.01, err_msg='aer2geodetic {}'.format( pm.aer2geodetic(taz, tel, tsrange, tlat, tlon, talt))) assert_allclose(pm.ecef2aer(tx, ty, tz, tlat, tlon, talt), (ec2az, ec2el, ec2rn), rtol=0.01, err_msg='ecef2aer {}'.format( pm.ecef2aer(a2x, a2y, a2z, tlat, tlon, talt))) #%% assert_allclose(pm.enu2aer(te, tn, tu), (e2az, e2el, e2rn), rtol=0.01, err_msg='enu2aer: ' + str(pm.enu2aer(te, tn, tu))) assert_allclose(pm.ned2aer(tn, te, -tu), (e2az, e2el, e2rn), rtol=0.01, err_msg='enu2aer: ' + str(pm.enu2aer(te, tn, tu))) assert_allclose(pm.enu2geodetic(te, tn, tu, tlat, tlon, talt), (lat2, lon2, alt2), rtol=0.01, err_msg='enu2geodetic: ' + str(pm.enu2geodetic(te, tn, tu, tlat, tlon, talt))) assert_allclose(pm.ned2geodetic(tn, te, -tu, tlat, tlon, talt), (lat2, lon2, alt2), rtol=0.01, err_msg='enu2geodetic: ' + str(pm.enu2geodetic(te, tn, tu, tlat, tlon, talt))) assert_allclose(pm.enu2ecef(te, tn, tu, tlat, tlon, talt), (e2x, e2y, e2z), rtol=0.01, err_msg='enu2ecef: ' + str(pm.enu2ecef(te, tn, tu, tlat, tlon, talt))) assert_allclose( pm.ned2ecef(tn, te, -tu, tlat, tlon, talt), (e2x, e2y, e2z), rtol=0.01, err_msg='ned2ecef: ' + str(pm.ned2ecef(tn, te, -tu, tlat, tlon, talt)))
svrange = svBiasRange[0] D = xarray.open_dataset(fobs, group='OBS', autoclose=True) rx_xyz = D.position C1 = D.sel(sv=prn)['C1'].values obstimes64 = D.time.values T = np.array([Timestamp(t).to_pydatetime() for t in obstimes64]) aer = np.array(pyGnss.getSatellitePosition(rx_xyz, prn, times + timedelta(seconds=17), fnav, cs='aer',dtype='georinex')) satxyz = np.array(pyGnss.getSatellitePosition(rx_xyz, prn, times + timedelta(seconds=17), fnav, cs='xyz',dtype='georinex')) rho = np.sqrt( np.square(satxyz[0]-rx_xyz[0]) + np.square(satxyz[1]-rx_xyz[1]) + np.square(satxyz[2]-rx_xyz[2]) ) #- svrange #New satxyz = gpsSatPosition(fnav, times, sv=prn) rec_lat, rec_lon, rec_alt = ecef2geodetic(rx_xyz[0], rx_xyz[1], rx_xyz[2]) A,E,R = ecef2aer(satxyz[0], satxyz[1], satxyz[2], rec_lat, rec_lon, rec_alt) plt.figure(figsize=(10,7)) plt.title(prn) plt.plot(dt[idsv], rho, 'b') plt.plot(T, C1, 'r') #plt.figure(figsize=(10,7)) #plt.plot(dt[idsv], el[idsv], '.b') #plt.plot(dt[idsv], aer[1], 'r', lw=3) # plt.figure(figsize=(10,7)) plt.plot(dt[idsv], el[idsv]-aer[1], 'r') plt.plot(dt[idsv], el[idsv]-E, 'k') # #plt.figure(figsize=(10,7))
def test_ecef2aer(xyz, lla, aer): assert pm.ecef2aer(*xyz, *lla) == approx(aer) rlla = (radians(lla[0]), radians(lla[1]), lla[2]) raer = (radians(aer[0]), radians(aer[1]), aer[2]) assert pm.ecef2aer(*xyz, *rlla, deg=False) == approx(raer)
def map_target(tx, rx, az, el, rf, dop, wavelength): """ Find the scatter location given tx location, rx location, total rf distance, and target angle-of-arrival using the 'WGS84' Earth model. Also determines the bistatic velocity vector and bistatic radar wavelength. Parameters ---------- tx : float np.array [latitude, longitude, altitude] of tx array in degrees and kilometers rx : float np.array [latitude, longitude, altitude] of rx array in degrees and kilometers az : float np.array angle-of-arrival azimuth in degrees el : float np.array angle-of-arrival elevation in degrees rf : float np.array total rf path distance rf = c * tau in kilometers dop : float np.array doppler shift in hertz wavelength : float radar signal center wavelength Returns ------- sx : float np.array [latitude, longitude, altitude] of scatter in degrees and kilometers sa : float np.array [azimuth, elevation, slant range] of scatter in degrees and kilometers sv : float np.array [azimuth, elevation, velocity] the bistatic Doppler velocity vector in degrees and kilometers. Coordinates given in the scattering targets local frame (azimuth from North, elevation up from the plane normal to zenith, Doppler [Hz] * lambda / (2 cos(e/2)) ) Notes ----- tx : transmitter location rx : receiver location sx : scatter location gx : geometric center of Earth, origin u_rt : unit vector rx to tx u_rs : unit vector rx to sx u_gt : unit vector gx to tx u_gr : unit vector gx to rx u_gs : unit vector gx to sx """ # Initialize output arrays sx = np.zeros((3, len(rf)), dtype=float) sa = np.zeros((3, len(rf)), dtype=float) sv = np.zeros((3, len(rf)), dtype=float) # Setup variables in correct units for pymap3d rf = rf * 1.0e3 az = np.where(az < 0.0, az + 360.0, az) az = np.deg2rad(az) el = np.deg2rad(np.abs(el)) # Determine the slant range, r bx1, by1, bz1 = pm.geodetic2ecef(rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) v_gr = np.array([bx1, by1, bz1]) bx2, by2, bz2 = pm.geodetic2ecef(tx[0], tx[1], tx[2], ell=pm.Ellipsoid("wgs84"), deg=True) v_gt = np.array([bx2, by2, bz2]) raz, rel, b = pm.ecef2aer(bx2, by2, bz2, rx[0], rx[1], rx[2], ell=pm.Ellipsoid("wgs84"), deg=True) u_rt = np.array([ np.sin(np.deg2rad(raz)) * np.cos(np.deg2rad(rel)), np.cos(np.deg2rad(raz)) * np.cos(np.deg2rad(rel)), np.sin(np.deg2rad(rel)) ]) el -= relaxation_elevation(el, rf, az, b, u_rt) u_rs = np.array( [np.sin(az) * np.cos(el), np.cos(az) * np.cos(el), np.sin(el)]) r = (rf**2 - b**2) / (2 * (rf - b * np.dot(u_rt, u_rs))) # WGS84 Model for lat, long, alt sx[:, :] = pm.aer2geodetic(np.rad2deg(az), np.rad2deg(el), np.abs(r), np.repeat(rx[0], len(az)), np.repeat(rx[1], len(az)), np.repeat(rx[2], len(az)), ell=pm.Ellipsoid("wgs84"), deg=True) # Determine the bistatic Doppler velocity vector x, y, z = pm.geodetic2ecef(sx[0, :], sx[1, :], sx[2, :], ell=pm.Ellipsoid('wgs84'), deg=True) v_gs = np.array([x, y, z]) v_bi = (-1 * v_gs.T + v_gt / 2.0 + v_gr / 2.0).T u_bi = v_bi / np.linalg.norm(v_bi, axis=0) v_sr = (v_gr - v_gs.T).T u_sr = v_sr / np.linalg.norm(v_sr, axis=0) radar_wavelength = wavelength / np.abs( 2.0 * np.einsum('ij,ij->j', u_sr, u_bi)) # doppler_sign = np.sign(dop) # 1 for positive, -1 for negative, and 0 for zero doppler_sign = np.where( dop >= 0, 1, -1) # 1 for positive, -1 for negative, and 0 for zero vaz, vel, _ = pm.ecef2aer(doppler_sign * u_bi[0, :] + x, doppler_sign * u_bi[1, :] + y, doppler_sign * u_bi[2, :] + z, sx[0, :], sx[1, :], sx[2, :], ell=pm.Ellipsoid("wgs84"), deg=True) # Convert back to conventional units sx[2, :] /= 1.0e3 az = np.rad2deg(az) el = np.rad2deg(el) sa[:, :] = np.array([az, el, r / 1.0e3]) sv[:, :] = np.array([vaz, vel, dop * radar_wavelength]) return sx, sa, sv