def test_ecef_geo_inverse(self): dec = 3 y = n.array((90.0, 0.0, 0.0)) x = coord.geodetic2ecef(y[0], y[1], y[2]) y_ref = coord.ecef2geodetic(x[0], x[1], x[2]) nt.assert_array_almost_equal(y, y_ref, decimal=dec) y = n.array((-90.0, 0.0, 0.0)) x = coord.geodetic2ecef(y[0], y[1], y[2]) y_ref = coord.ecef2geodetic(x[0], x[1], x[2]) nt.assert_array_almost_equal(y, y_ref, decimal=dec) y = n.array((0.0, 0.0, 0.0)) x = coord.geodetic2ecef(y[0], y[1], y[2]) y_ref = coord.ecef2geodetic(x[0], x[1], x[2]) nt.assert_array_almost_equal(y, y_ref, decimal=dec) y = n.array((0.0, 90.0, 0.0)) x = coord.geodetic2ecef(y[0], y[1], y[2]) y_ref = coord.ecef2geodetic(x[0], x[1], x[2]) nt.assert_array_almost_equal(y, y_ref, decimal=dec) y = n.array((90.0, 0.0, 100.0)) x = coord.geodetic2ecef(y[0], y[1], y[2]) y_ref = coord.ecef2geodetic(x[0], x[1], x[2]) nt.assert_array_almost_equal(y, y_ref, decimal=dec)
def test_ecef2geodetic(self): dec = 3 x = coord.ecef2geodetic(0.0, 0.0, coord.b) y = n.array((90.0, 0.0, 0.0)) nt.assert_array_almost_equal(x, y, decimal=dec) x = coord.ecef2geodetic(0.0, 0.0, -coord.b) y = n.array((-90.0, 0.0, 0.0)) nt.assert_array_almost_equal(x, y, decimal=dec) x = coord.ecef2geodetic(coord.a, 0.0, 0.0) y = n.array((0.0, 0.0, 0.0)) nt.assert_array_almost_equal(x, y, decimal=dec) x = coord.ecef2geodetic(0.0, coord.a, 0.0) y = n.array((0.0, 90.0, 0.0)) nt.assert_array_almost_equal(x, y, decimal=dec) x = coord.ecef2geodetic(0.0, 0.0, coord.b + 100.) y = n.array((90.0, 0.0, 100.0)) nt.assert_array_almost_equal(x, y, decimal=dec)
def ray_trace(dn=datetime(2016, 6, 21, 12, 00), f=233e6, lat=e3d._tx[0].lat, lon=e3d._tx[0].lon, elevation=30.0, az=180.0, fpref="", plot=False): np = 1000 alts = n.linspace(0, 4000, num=np) distance = n.linspace(0, 4000, num=np) ne = n.zeros(np) ne2 = n.zeros(np) dnex = n.zeros(np) dtheta = n.zeros(np) dalt = n.zeros(np) dney = n.zeros(np) dnez = n.zeros(np) xyz_prev = 0.0 px = n.zeros(np) dk = n.zeros(np) py = n.zeros(np) pz = n.zeros(np) p0x = n.zeros(np) p0y = n.zeros(np) p0z = n.zeros(np) # initial direction and position k = coord.azel_ecef(lat, lon, 10e3, az, elevation) k0 = k p = coord.geodetic2ecef(lat, lon, 10e3) pe = coord.geodetic2ecef(lat, lon, 10e3) p0 = coord.geodetic2ecef(lat, lon, 10e3) dh = 4e3 vg = 1.0 p_orig = p ray_time = 0.0 for ai, a in enumerate(alts): p = p + k * dh * vg p0 = p0 + k0 * dh ray_time += dh / c.c dpx = p + n.array([1.0, 0.0, 0.0]) * dh dpy = p + n.array([0.0, 1.0, 0.0]) * dh dpz = p + n.array([0.0, 0.0, 1.0]) * dh llh = coord.ecef2geodetic(p[0], p[1], p[2]) llh_1 = coord.ecef2geodetic(p0[0], p0[1], p0[2]) dalt[ai] = llh_1[2] - llh[2] if llh[2] / 1e3 > 1900: break alts[ai] = llh[2] / 1e3 pt = Point(dn, llh[0], llh[1], llh[2] / 1e3) pt.run_iri() if pt.ne > 0.0: ne[ai] = pt.ne * 1e6 f_p = 8.98 * n.sqrt(ne[ai]) v_g = n.sqrt(1.0 - (f_p / f)**2.0) else: ne[ai] = 0.0 llh = coord.ecef2geodetic(dpx[0], dpx[1], dpx[2]) pt = Point(dn, llh[0], llh[1], llh[2] / 1e3) pt.run_iri() if pt.ne > 0.0: dnex[ai] = (ne[ai] - pt.ne * 1e6) / dh else: dnex[ai] = 0.0 llh = coord.ecef2geodetic(dpy[0], dpy[1], dpy[2]) pt = Point(dn, llh[0], llh[1], llh[2] / 1e3) pt.run_iri() if pt.ne > 0.0: dney[ai] = (ne[ai] - pt.ne * 1e6) / dh else: dney[ai] = 0.0 llh = coord.ecef2geodetic(dpz[0], dpz[1], dpz[2]) pt = Point(dn, llh[0], llh[1], llh[2] / 1e3) pt.run_iri() if pt.ne > 0.0: dnez[ai] = (ne[ai] - pt.ne * 1e6) / dh else: dnez[ai] = 0.0 grad = n.array([dnex[ai], dney[ai], dnez[ai]]) px[ai] = p[0] py[ai] = p[1] pz[ai] = p[2] p0x[ai] = p0[0] p0y[ai] = p0[1] p0z[ai] = p0[2] # print(ai) dk[ai] = n.arccos( n.dot(k0, k) / (n.sqrt(n.dot(k0, k0)) * n.sqrt(n.dot(k, k)))) # no bending if gradient too small if n.dot(grad, grad) > 100.0: grad1 = grad / n.sqrt(n.dot(grad, grad)) p2 = p + k * dh llh = coord.ecef2geodetic(p2[0], p2[1], p2[2]) pt = Point(dn, llh[0], llh[1], llh[2] / 1e3) pt.run_iri() if pt.ne > 0.0: ne2 = pt.ne * 1e6 else: ne2 = 0.0 f0 = 8.98 * n.sqrt(ne[ai]) n0 = n.sqrt(1.0 - (f0 / f)**2.0) f1 = 8.98 * n.sqrt(ne2) n1 = n.sqrt(1.0 - (f1 / f)**2.0) theta0 = n.arccos( n.dot(grad, k) / (n.sqrt(n.dot(grad, grad)) * n.sqrt(n.dot(k, k)))) # angle cannot be over 90 if theta0 > n.pi / 2.0: theta0 = n.pi - theta0 sin_theta_1 = (n0 / n1) * n.sin(theta0) dtheta[ai] = 180.0 * n.arcsin( sin_theta_1) / n.pi - 180.0 * theta0 / n.pi # print("n0/n1 %1.10f theta0 %1.2f theta1-theta0 %1.10f"%(n0/n1,180.0*theta0/n.pi,dtheta[ai])) cos_theta_1 = n.sqrt(1.0 - sin_theta_1**2.0) k_ref = (n0 / n1) * k + ( (n0 / n1) * n.cos(theta0) - cos_theta_1) * grad1 # normalize k_ref / n.sqrt(n.dot(k_ref, k_ref)) k = k_ref angle = n.arccos( n.dot(grad, k) / n.sqrt(n.dot(grad, grad)) * n.sqrt(n.dot(k, k))) los_time = n.sqrt(n.dot(p_orig - p, p_orig - p)) / c.c excess_ionospheric_delay = ray_time - los_time print("Excess propagation time %1.20f mus" % ((1e6 * (ray_time - los_time)))) theta = n.arccos( n.dot(k0, k) / (n.sqrt(n.dot(k0, k0)) * n.sqrt(n.dot(k, k)))) theta_p = n.arccos( n.dot(p0, p) / (n.sqrt(n.dot(p0, p0)) * n.sqrt(n.dot(p, p)))) llh0 = coord.ecef2geodetic(px[ai - 2], py[ai - 2], pz[ai - 2]) llh1 = coord.ecef2geodetic(p0x[ai - 2], p0y[ai - 2], p0z[ai - 2]) print("d_coord") print(llh0 - llh1) if plot: print(p0 - p) print(180.0 * theta_p / n.pi) fig = plt.figure(figsize=(14, 8)) plt.clf() plt.subplot(131) plt.title("Elevation=%1.0f" % (elevation)) plt.plot(n.sqrt((p0x - px)**2.0 + (p0y - py)**2.0 + (p0z - pz)**2.0), alts, label="Total error") plt.plot(dalt, alts, label="Altitude error") plt.ylim([0, 1900]) # plt.xlim([-50,800.0]) plt.grid() plt.legend() plt.xlabel("Position error (m)") plt.ylabel("Altitude km") plt.subplot(132) plt.plot(dtheta * 1e6, alts) # plt.plot(1e6*180.0*dk/n.pi,alts) plt.xlabel("Ray-bending ($\mu$deg/km)") plt.ylabel("Altitude km") plt.title("Total error=%1.2g (deg)" % (180.0 * theta_p / n.pi)) plt.ylim([0, 1900]) plt.subplot(133) plt.plot(ne, alts) plt.xlabel("$N_{\mathrm{e}}$ ($\mathrm{m}^{-3}$)") plt.ylabel("Altitude km") plt.ylim([0, 1900]) # ax.plot(px,py,pz) plt.tight_layout() plt.savefig("ref-%s-%d-%d.png" % (fpref, f / 1e6, elevation)) plt.close() return (p0, p, 180.0 * theta_p / n.pi, excess_ionospheric_delay)
def ray_trace_error(dn=datetime(2016, 6, 21, 12, 00), f=233e6, lat=e3d._tx[0].lat, lon=e3d._tx[0].lon, elevation=30.0, az=180.0, fpref="", ionosphere=False, error_std=0.05, plot=False): np = 2000 alts = n.repeat(1e99, np) distance = n.linspace(0, 4000, num=np) ne = n.zeros(np) ne2 = n.zeros(np) dtheta = n.zeros(np) dalt = n.zeros(np) dnex = n.zeros(np) dney = n.zeros(np) dnez = n.zeros(np) xyz_prev = 0.0 dk = n.zeros(np) px = n.zeros(np) py = n.zeros(np) pz = n.zeros(np) t_vec = n.zeros(np) t_i_vec = n.zeros(np) k_vecs = [] # initial direction and position k = coord.azel_ecef(lat, lon, 10e3, az, elevation) k0 = k p = coord.geodetic2ecef(lat, lon, 10e3) dh = 4e3 dt = 20e-6 # correlated errors std=1, 100 km correlation length scale_length = 40.0 ne_errors_x = n.convolve( n.repeat(1.0 / n.sqrt(scale_length), scale_length), n.random.randn(10000)) ne_errors_y = n.convolve( n.repeat(1.0 / n.sqrt(scale_length), scale_length), n.random.randn(10000)) ne_errors_z = n.convolve( n.repeat(1.0 / n.sqrt(scale_length), scale_length), n.random.randn(10000)) p_orig = p ray_time = 0.0 v_c = c.c for ai, a in enumerate(alts): # go forward in time dhp = v_c * dt p = p + k * dhp ray_time += dt print(ray_time * 1e6) t_vec[ai + 1] = dt k_vecs.append(k) dpx = p + n.array([1.0, 0.0, 0.0]) * dh dpy = p + n.array([0.0, 1.0, 0.0]) * dh dpz = p + n.array([0.0, 0.0, 1.0]) * dh llh = coord.ecef2geodetic(p[0], p[1], p[2]) if llh[2] / 1e3 > 2100: break alts[ai] = llh[2] / 1e3 pt = Point(dn, llh[0], llh[1], llh[2] / 1e3) pt.run_iri() if pt.ne > 0.0: ne[ai] = pt.ne * (1.0 + error_std * ne_errors_x[ai]) * 1e6 if ionosphere: f0 = 8.98 * n.sqrt(ne[ai]) f_p = 8.98 * n.sqrt(ne[ai]) # update group velocity v_c = c.c * n.sqrt(1.0 - (f0 / f)**2.0) else: ne[ai] = 0.0 llh = coord.ecef2geodetic(dpx[0], dpx[1], dpx[2]) pt = Point(dn, llh[0], llh[1], llh[2] / 1e3) pt.run_iri() if pt.ne > 0.0: dnex[ai] = (ne[ai] - pt.ne * (1.0 + error_std * ne_errors_x[ai]) * 1e6) / dh else: dnex[ai] = 0.0 llh = coord.ecef2geodetic(dpy[0], dpy[1], dpy[2]) pt = Point(dn, llh[0], llh[1], llh[2] / 1e3) pt.run_iri() if pt.ne > 0.0: dney[ai] = (ne[ai] - pt.ne * (1.0 + error_std * ne_errors_x[ai]) * 1e6) / dh else: dney[ai] = 0.0 llh = coord.ecef2geodetic(dpz[0], dpz[1], dpz[2]) pt = Point(dn, llh[0], llh[1], llh[2] / 1e3) pt.run_iri() if pt.ne > 0.0: dnez[ai] = (ne[ai] - pt.ne * (1.0 + error_std * ne_errors_x[ai]) * 1e6) / dh else: dnez[ai] = 0.0 grad = n.array([dnex[ai], dney[ai], dnez[ai]]) px[ai] = p[0] py[ai] = p[1] pz[ai] = p[2] dk[ai] = n.arccos( n.dot(k0, k) / (n.sqrt(n.dot(k0, k0)) * n.sqrt(n.dot(k, k)))) # no bending if gradient too small if n.dot(grad, grad) > 100.0 and ionosphere: grad1 = grad / n.sqrt(n.dot(grad, grad)) p2 = p + k * dh llh = coord.ecef2geodetic(p2[0], p2[1], p2[2]) pt = Point(dn, llh[0], llh[1], llh[2] / 1e3) pt.run_iri() if pt.ne > 0.0: ne2 = pt.ne * (1.0 + error_std * ne_errors_x[ai]) * 1e6 else: ne2 = 0.0 f0 = 8.98 * n.sqrt(ne[ai]) n0 = n.sqrt(1.0 - (f0 / f)**2.0) f1 = 8.98 * n.sqrt(ne2) n1 = n.sqrt(1.0 - (f1 / f)**2.0) theta0 = n.arccos( n.dot(grad, k) / (n.sqrt(n.dot(grad, grad)) * n.sqrt(n.dot(k, k)))) # angle cannot be over 90 if theta0 > n.pi / 2.0: theta0 = n.pi - theta0 sin_theta_1 = (n0 / n1) * n.sin(theta0) dtheta[ai] = 180.0 * n.arcsin( sin_theta_1) / n.pi - 180.0 * theta0 / n.pi # print("n0/n1 %1.10f theta0 %1.2f theta1-theta0 %1.10f"%(n0/n1,180.0*theta0/n.pi,dtheta[ai])) cos_theta_1 = n.sqrt(1.0 - sin_theta_1**2.0) k_ref = (n0 / n1) * k + ( (n0 / n1) * n.cos(theta0) - cos_theta_1) * grad1 # normalize k_ref / n.sqrt(n.dot(k_ref, k_ref)) k = k_ref angle = n.arccos( n.dot(grad, k) / n.sqrt(n.dot(grad, grad)) * n.sqrt(n.dot(k, k))) return (t_vec, px, py, pz, alts, ne, k_vecs)
def __init__(self,N=1000,R_e=6378e3,h_e=100e3,plot=False): self.N=N self.R_e=R_e # get uniformly distributed points on a sphere # these will act ast grid points points=fibonacci_sphere(samples=N)*(R_e+h_e) # triangulate before shifting points onto a geoid tri = Delaunay(points) # shift points to fit geoid if True: points2=n.copy(points) llhs=[] for p in range(points.shape[0]): if p == 0: llh=[0,0,0] points[p,:]=n.array([0,0,0]) else: llh=coord.ecef2geodetic(points[p,0],points[p,1],points[p,2]) points2[p,:]=coord.geodetic2ecef(llh[0],llh[1],h_e) llhs.append(llh) llhs=n.array(llhs) points=points2 # triangulate the grid, so that we know # the neighbouring points # the list "tri" contains the wire mesh # TBD: make sure that there are no duplicated wires! s=n.copy(tri.simplices) # these are the triangles tris = [] # these are unique current carrying elements of the mesh wires={} wire_num=0 # these are the connections between node points, in order to allow us to regularize current continuity. self.connections={} for i in range(s.shape[0]): pidx=s[i,:] if 0 in pidx: pidx=n.setdiff1d(pidx,[0]) tri={} tri["edges"]=[[pidx[0],pidx[1]],[pidx[0],pidx[2]],[pidx[1],pidx[2]]] id_pairs=[[0,1],[0,2],[1,2]] wire_ids=[] for id_pair in id_pairs: e0=n.min([pidx[id_pair[0]],pidx[id_pair[1]]]) e1=n.max([pidx[id_pair[0]],pidx[id_pair[1]]]) self.add_connection(e0,e1) wire_id = "%d-%d"%(e0,e1) wire_ids.append(wire_id) if wire_id not in wires.keys(): del_l=points[e0,:]-points[e1,:] r=points[e0,:]+0.5*del_l llh=coord.ecef2geodetic(r[0],r[1],r[2]) east=coord.enu2ecef(llh[0],llh[1],0.0,1.0,0,0) north=coord.enu2ecef(llh[0],llh[1],0.0,0,1.0,0) wires[wire_id]={"del_l":del_l, "del_e":n.dot(del_l/n.linalg.norm(del_l),east), "del_n":n.dot(del_l/n.linalg.norm(del_l),north), "r":r, "llh":llh, "wire_num":wire_num, "e0":e0, "e1":e1} wire_num+=1 else: pass tri["wire_ids"]=wire_ids tris.append(tri) else: print("middle point not in tri!") self.llhs=llhs self.tris=tris self.points=points self.wires=wires
def create_tracklet(o, radar, t_obs, hdf5_out=True, ccsds_out=True, dname="./tracklets", noise=False, dx=10.0, dv=10.0, dt=0.01, ignore_elevation_thresh=False): '''Simulate tracks of objects. ionospheric limit is a lower limit on precision after ionospheric corrections ''' if noise: noise = 1.0 else: noise = 0.0 # TDB, kludge, this should be allowed to change as a function of time bw = radar._tx[0].tx_bandwidth txlen = radar._tx[0].pulse_length * 1e6 # pulse length in microseconds ipp = radar._tx[0].ipp # pulse length in microseconds n_ipp = int(radar._tx[0].n_ipp) # pulse length in microseconds rfun, dopfun = debris.precalculate_dr(txlen, bw, ipp=ipp, n_ipp=n_ipp) t0_unix = dpt.jd_to_unix(dpt.mjd_to_jd(o.mjd0)) if debug_low: for tx in radar._tx: print("TX %s" % (tx.name)) for rx in radar._tx: print("RX %s" % (rx.name)) rx_p = [] tx_p = [] for tx in radar._tx: tx_p.append(coord.geodetic2ecef(tx.lat, tx.lon, tx.alt)) for rxi, rx in enumerate(radar._rx): rx_p.append(coord.geodetic2ecef(rx.lat, rx.lon, rx.alt)) ecefs = o.get_orbit(t_obs) state = o.get_state(t_obs) ecefs_p_dt = o.get_orbit(t_obs + dt) ecefs_m_dt = o.get_orbit(t_obs - dt) # velocity in ecef ecef_vel = (0.5 * ((ecefs_p_dt - ecefs) + (ecefs - ecefs_m_dt)) / dt) # linearized error estimates for ecef state vector error std_dev, when three or more # delays and doppl er shifts are measured ecef_stdevs = [] meas = [] for tx in radar._tx: ecef_stdevs.append({"time_idx": [], "m_time": [], "ecef_stdev": []}) for tx in radar._tx: m_rx = [] for rx in radar._rx: m_rx.append({ "time_idx": [], "m_time": [], "m_delay": [], "m_delay_std": [], "m_range": [], "m_range_std": [], "m_range_rate": [], "m_range_rate_std": [], "m_doppler": [], "m_doppler_std": [], "gain_tx": [], "gain_rx": [], "enr": [], "true_state": [], "true_time": [], "g_lat": [], "g_lon": [] }) meas.append(m_rx) # largest possible number of state vector measurements n_state_meas = len(radar._tx) * len(radar._rx) # jacobian for error covariance calc J = n.zeros([2 * n_state_meas, 6]) Sigma_Lin = n.zeros([2 * n_state_meas, 2 * n_state_meas]) # error standard deviation for state vector estimates at each position state_vector_errors = n.zeros([6, len(t_obs)]) # go through all times for ti, t in enumerate(t_obs): p = n.array([ecefs[0, ti], ecefs[1, ti], ecefs[2, ti]]) p_p = n.array( [ecefs_p_dt[0, ti], ecefs_p_dt[1, ti], ecefs_p_dt[2, ti]]) p_m = n.array( [ecefs_m_dt[0, ti], ecefs_m_dt[1, ti], ecefs_m_dt[2, ti]]) # for linearized state vector error determination p_dx0 = n.array([ecefs[0, ti] + dx, ecefs[1, ti], ecefs[2, ti]]) p_dx1 = n.array([ecefs[0, ti], ecefs[1, ti] + dx, ecefs[2, ti]]) p_dx2 = n.array([ecefs[0, ti], ecefs[1, ti], ecefs[2, ti] + dx]) # doppler error comes from linear least squares # initialize jacobian J[:, :] = 0.0 Sigma_Lin[:, :] = 0.0 state_meas_idx = 0 # go through all transmitters for txi, tx in enumerate(radar._tx): pos_vec_tx = -tx_p[txi] + p pos_vec_tx_p = -tx_p[txi] + p_p pos_vec_tx_m = -tx_p[txi] + p_m # for linearized errors pos_vec_tx_dx0 = -tx_p[txi] + p_dx0 pos_vec_tx_dx1 = -tx_p[txi] + p_dx1 pos_vec_tx_dx2 = -tx_p[txi] + p_dx2 # incident k-vector k_inc = -2.0 * n.pi * pos_vec_tx / n.linalg.norm( pos_vec_tx) / tx.wavelength elevation_tx = 90.0 - coord.angle_deg(tx_p[txi], pos_vec_tx) if elevation_tx > tx.el_thresh or ignore_elevation_thresh: k0 = tx.point_ecef( pos_vec_tx) # we are pointing at the object when tracking gain_tx = tx.beam.gain(k0) # get antenna gain range_tx = n.linalg.norm(pos_vec_tx) range_tx_p = n.linalg.norm(pos_vec_tx_p) range_tx_m = n.linalg.norm(pos_vec_tx_m) range_tx_dx0 = n.linalg.norm(pos_vec_tx_dx0) range_tx_dx1 = n.linalg.norm(pos_vec_tx_dx1) range_tx_dx2 = n.linalg.norm(pos_vec_tx_dx2) tx_to_target_time = range_tx / c.c # go through all receivers for rxi, rx in enumerate(radar._rx): pos_vec_rx = -rx_p[rxi] + p pos_vec_rx_p = -rx_p[rxi] + p_p pos_vec_rx_m = -rx_p[rxi] + p_m # rx k-vector k_rec = 2.0 * n.pi * pos_vec_rx / n.linalg.norm( pos_vec_rx) / tx.wavelength # scattered k-vector k_scat = k_rec - k_inc # for linearized pos error pos_vec_rx_dx0 = -rx_p[rxi] + p_dx0 pos_vec_rx_dx1 = -rx_p[rxi] + p_dx1 pos_vec_rx_dx2 = -rx_p[rxi] + p_dx2 elevation_rx = 90.0 - coord.angle_deg( rx_p[rxi], pos_vec_rx) if elevation_rx > rx.el_thresh or ignore_elevation_thresh: k0 = rx.point_ecef( pos_vec_rx ) # we are pointing at the object when tracking gain_rx = rx.beam.gain(k0) # get antenna gain range_rx = n.linalg.norm(pos_vec_rx) range_rx_p = n.linalg.norm(pos_vec_rx_p) range_rx_m = n.linalg.norm(pos_vec_rx_m) range_rx_dx0 = n.linalg.norm(pos_vec_rx_dx0) range_rx_dx1 = n.linalg.norm(pos_vec_rx_dx1) range_rx_dx2 = n.linalg.norm(pos_vec_rx_dx2) target_to_rx_time = range_rx / c.c # SNR of object at measured location enr_rx = debris.hard_target_enr( gain_tx, gain_rx, tx.wavelength, tx.tx_power, range_tx, range_rx, o.diam, bandwidth=tx. coh_int_bandwidth, # coherent integration bw rx_noise_temp=rx.rx_noise) if enr_rx > 1e8: enr_rx = 1e8 if enr_rx < 0.1: enr_rx = 0.1 #print("snr %1.2f"%(enr_rx)) dr = 10.0**(rfun(n.log10(enr_rx))) ddop = 10.0**(dopfun(n.log10(enr_rx))) # Unknown doppler shift due to ionosphere can be up to 0.1 Hz, # estimate based on typical GNU Ionospheric tomography receiver phase curves. if ddop < 0.1: ddop = 0.1 dr = n.sqrt(dr**2.0 + iono_errfun(range_tx / 1e3)** 2.0) # add ionospheric error if dr < o.diam: # if object diameter is larger than range error, make it at least as big as target dr = o.diam r0 = range_tx + range_rx rp = range_tx_p + range_rx_p rm = range_tx_m + range_rx_m range_rate_d = 0.5 * ( (rp - r0) + (r0 - rm)) / dt # symmetric numerical derivative # doppler (m/s) using scattering k-vector range_rate = n.dot( pos_vec_rx / range_rx, state[3:6, ti]) + n.dot( pos_vec_tx / range_tx, state[3:6, ti]) doppler = range_rate / tx.wavelength # print("rr1 %1.1f rr2 %1.1f"%(range_rate_d,range_rate)) doppler_k = n.dot(k_scat, ecef_vel[:, ti]) / 2.0 / n.pi range_rate_k = doppler_k * tx.wavelength # for linearized errors, range rate at small perturbations to state vector velocity parameters range_rate_k_dv0 = tx.wavelength * n.dot( k_scat, ecef_vel[:, ti] + n.array([dv, 0, 0])) / 2.0 / n.pi range_rate_k_dv1 = tx.wavelength * n.dot( k_scat, ecef_vel[:, ti] + n.array([0, dv, 0])) / 2.0 / n.pi range_rate_k_dv2 = tx.wavelength * n.dot( k_scat, ecef_vel[:, ti] + n.array([0, 0, dv])) / 2.0 / n.pi # full range for error calculation, with small perturbations to position state full_range_dx0 = range_rx_dx0 + range_tx_dx0 full_range_dx1 = range_rx_dx1 + range_tx_dx1 full_range_dx2 = range_rx_dx2 + range_tx_dx2 if enr_rx > tx.enr_thresh: # calculate jacobian row for state vector errors # range J[2 * state_meas_idx, 0:3] = n.array([(full_range_dx0 - r0) / dx, (full_range_dx1 - r0) / dx, (full_range_dx2 - r0) / dx]) # range inverse variance Sigma_Lin[2 * state_meas_idx, 2 * state_meas_idx] = 1.0 / dr**2.0 # range-rate J[2 * state_meas_idx + 1, 3:6] = n.array([ (range_rate_k_dv0 - range_rate_k) / dv, (range_rate_k_dv1 - range_rate_k) / dv, (range_rate_k_dv2 - range_rate_k) / dv ]) # range-rate inverse variance Sigma_Lin[2 * state_meas_idx + 1, 2 * state_meas_idx + 1] = 1.0 / (tx.wavelength * ddop)**2.0 state_meas_idx += 1 # detection! if debug_low: print( "rx %d tx el %1.2f rx el %1.2f gain_tx %1.2f gain_rx %1.2f enr %1.2f rr %1.2f prop time %1.6f dr %1.2f" % (rxi, elevation_tx, elevation_rx, gain_tx, gain_rx, enr_rx, range_rate, tx_to_target_time, dr)) # ground foot point in geodetic llh = coord.ecef2geodetic(p[0], p[1], p[2]) # time is time of transmit pulse meas[txi][rxi]["time_idx"].append(ti) meas[txi][rxi]["m_time"].append(t + t0_unix) meas[txi][rxi]["m_range"].append( (range_tx + range_rx) / 1e3 + noise * n.random.randn() * dr / 1e3) meas[txi][rxi]["m_range_std"].append(dr / 1e3) rr_std = c.c * ddop / radar._tx[ txi].freq / 2.0 / 1e3 meas[txi][rxi]["m_range_rate"].append( range_rate / 1e3 + noise * n.random.randn() * rr_std) # 0.1 m/s error due to ionosphere meas[txi][rxi]["m_range_rate_std"].append(rr_std) meas[txi][rxi]["m_doppler"].append( doppler + noise * n.random.randn() * ddop / 1e3) meas[txi][rxi]["m_doppler_std"].append(ddop) meas[txi][rxi]["m_delay"].append( tx_to_target_time + target_to_rx_time) meas[txi][rxi]["g_lat"].append(llh[0]) meas[txi][rxi]["g_lon"].append(llh[1]) meas[txi][rxi]["enr"].append(enr_rx) meas[txi][rxi]["gain_tx"].append(gain_tx) meas[txi][rxi]["gain_rx"].append(gain_rx) true_state = n.zeros(6) true_state[3:6] = (0.5 * ((p_p - p) + (p - p_m)) / dt) / 1e3 true_state[0:3] = p / 1e3 meas[txi][rxi]["true_state"].append(true_state) meas[txi][rxi]["true_time"].append(t_obs[ti] + t0_unix) else: if debug_high: print("not detected: enr_rx {}".format(enr_rx)) else: if debug_high: print("not detected: elevation_rx {}".format( elevation_rx)) else: if debug_high: print("not detected: elevation_tx {}".format(elevation_tx)) # if more than three measurements of range and range-rate, then we maybe able to # observe true state. if so, calculate the linearized covariance matrix if state_meas_idx > 2: # use only the number of measurements that were good JJ = J[0:(2 * state_meas_idx), :] try: Sigma_post = n.linalg.inv( n.dot(n.dot(n.transpose(JJ), Sigma_Lin), JJ)) ecef_stdevs[txi]["time_idx"].append(ti) ecef_stdevs[txi]["m_time"].append(t) ecef_stdevs[txi]["ecef_stdev"].append(Sigma_post) except: print("Singular posterior covariance...") if debug_low: print(meas) fnames = write_tracklets(o, meas, radar, dname, hdf5_out=hdf5_out, ccsds_out=ccsds_out) return (meas, fnames, ecef_stdevs)