def find_bad_sensors(df, sensor_coords):
dists = dict()
for _, row in tqdm(df.iterrows(), total=len(df)):
geoAltitude = row['geoAltitude']
meas = json.loads(row['measurements'])
lat = row['latitude']
long = row['longitude']
xyz_gt = geo.latlon_to_cartesian(lat, long, geoAltitude)
receiver_coords = []
receiver_time = []
for m in meas:
sensor_id, timestamp, power = m
receiver_coords.append(sensor_coords[sensor_id])
receiver_time.append(timestamp)
d = math.sqrt(np.sum(np.square(xyz_gt - sensor_coords[sensor_id])))
if sensor_id not in dists:
dists[sensor_id] = []
dists[sensor_id].append(d)
bad_sensors = []
for k, v in dists.items():
if len(v) < 100:
bad_sensors.append(k)
elif np.min(v) > 25000:
bad_sensors.append(k)
return bad_sensors
def _get_TDoA(receiver_coords, pos_latlon, normalize=True):
pos = geo.latlon_to_cartesian(*pos_latlon)
TDoA = []
for rec in receiver_coords:
d = np.linalg.norm(rec - pos)
TDoA.append(d/geo.LIGHT_SPEED)
TDoA = np.array(TDoA)
if normalize:
TDoA -= TDoA.min()
return TDoA
def improve_3meas_points(mlat_4meas_time, mlat_4meas_coords_latlon, meas_time,
meas_sensor_coords, meas_received_time,
meas_altitude):
interpolated_coords_latlon, interpolated_time = \
interpolate_coords(mlat_4meas_time, mlat_4meas_coords_latlon, meas_time)
"""
Improve predicted coords for points with 3 or more measurements
"""
# smooth data applying low-pass filter
interpolated_coords_latlon = smooth_filter(interpolated_time,
interpolated_coords_latlon)
mlat_3meas_time = []
mlat_3meas_coords = []
for i in range(len(meas_time)):
receiver_coords = meas_sensor_coords[i]
receiver_time = meas_received_time[i]
receiver_power = []
if len(receiver_coords) < 3:
continue
receiver_time -= np.min(receiver_time)
if np.isnan(interpolated_coords_latlon[i, 0]):
continue
p = mlat_3points.calc(
receiver_coords, receiver_time, receiver_power, meas_altitude[i],
geo.latlon_to_cartesian(*interpolated_coords_latlon[i]),
IMPROVE_START_STEP, 0.000002)
if p is not None:
p_latlon = geo.cartesian_to_latlon(*p)
d = metrics._haversine_distance(p_latlon[0], p_latlon[1],
interpolated_coords_latlon[i, 0],
interpolated_coords_latlon[i, 1])
# remove coords when improved version is too far from interpolated version
if d < IMPROVE_DIST_THRESHOLD:
mlat_3meas_time.append(meas_time[i])
mlat_3meas_coords.append(p_latlon)
if len(mlat_3meas_time):
mlat_3meas_time = np.array(mlat_3meas_time)
mlat_3meas_coords = np.array(mlat_3meas_coords)
return combine_coords(mlat_3meas_time, mlat_3meas_coords,
mlat_4meas_time, mlat_4meas_coords_latlon)
else:
return mlat_4meas_time, mlat_4meas_coords_latlon
def calc(receiver_coords, timestamps, powers, altitude=None, start_pos=None, start_step = None, end_step = None):
"""
Iterative optimization method for points with 3 or more measurements
:param receiver_coords: sensor coords
:param timestamps: receiving time
:param powers:
:param altitude: baro altitude
:param start_pos: initial search coords
:param start_step: start lat/long step
:param end_step: finish lat/long step
:return: optimized coords
"""
if start_pos is None:
approx_pos = mlat_approx_by_power.calc(receiver_coords, timestamps, powers, altitude=altitude)
else:
approx_pos = start_pos
approx_pos_latlon = geo.cartesian_to_latlon(*approx_pos)
if altitude is not None:
approx_pos_latlon = (approx_pos_latlon[0], approx_pos_latlon[1], altitude)
target_TDoA = timestamps
best_TDoA_diff = np.linalg.norm(target_TDoA - _get_TDoA(receiver_coords, approx_pos_latlon))
best_pos_latlon = approx_pos_latlon
pos_latlon = approx_pos_latlon
updated = True
step = 0.005 if start_step is None else start_step
if end_step is None:
end_step = 0.0001
iter = 0
while updated and (step > end_step) and (iter < 100):
updated = False
for i in range(-1, 2):
for j in range(-1, 2):
if (i==0) and (j==0):
continue
pos2_latlon = np.array([pos_latlon[0] + i*step, pos_latlon[1] + j*step, pos_latlon[2]])
TDoA = _get_TDoA(receiver_coords, pos2_latlon)
diff = np.linalg.norm(target_TDoA - TDoA)
if diff < best_TDoA_diff:
best_TDoA_diff = diff
best_pos_latlon = pos2_latlon
updated = True
pos_latlon = best_pos_latlon
if not updated:
step /= 2
updated = True
iter += 1
if iter>=100:
return None
return geo.latlon_to_cartesian(*best_pos_latlon)
def get_train_sensor_coords_cartesian(filename):
sensors = pd.read_csv(filename)
id = sensors['serial'].values
coords = sensors[['latitude', 'longitude', 'height']].values
coords_cartesian = [geo.latlon_to_cartesian(*c) for c in coords]
return dict(zip(id, coords_cartesian))
def get_sensor_coords_cartesian(fixed=False):
sensors = get_sensors(fixed=fixed)
id = sensors['serial'].values
coords = sensors[['latitude', 'longitude', 'height']].values
coords_cartesian = [geo.latlon_to_cartesian(*c) for c in coords]
return dict(zip(id, coords_cartesian))