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)