def derive_gpst_meas(timestamp, obs_t, eph_t, nav_meas_tmp):
"""Process a single rover observation at a specific timestep,
producing sdiffs and
See: piksi_firmware/src/solution.c:time_matched_obs_thread.
Parameters
----------
timestamp : object
rover_obs_t : object
eph_t : object
nav_meas_tmp : object
Returns
----------
"""
nav_meas = obs_table_to_nav_measurement(timestamp, obs_t, eph_t)
if nav_meas_tmp is None:
return (nav_meas, None)
else:
# Update the observations (i.e., doppler and stuff and PVT).
nav_meas = update_nav_meas(nav_meas_tmp, nav_meas)
spp = calc_PVT(nav_meas.values())
return (nav_meas, spp)
def compute_ecef(pseudoranges, dops, sat_poss, sat_vels, t):
"""
Compute the receiver position if possible, otherwise a vector of NaNs.
Parameters
----------
pseudoranges : Series
Pseudoranges, may have NaNs, but the elements for the keys which are also
keys in dops, must not be NaN.
dops : dict
Dopplers, must not have NaNs.
sat_poss : dict
Satellite positions when this observation was received.
sat_vels : dict
Satellite velocities when this observation was received.
Returns
-------
array
The receiver position in ecef if there's enough info in the input to
compute it, otherwise an array of NaNs.
"""
if len(dops) < 4:
return (np.nan, np.nan, np.nan)
gpst = datetime2gpst(t)
nms = []
for itm in dops.iteritems():
sat = itm[0]
dop = itm[1]
pseudorange = pseudoranges[sat]
sat_pos = sat_poss[sat]
sat_vel = sat_vels[sat]
# TODO, make one of the pseudoranges/dops NaN or actually input and use it,
# instead of using either the corrected/raw for both corrected and raw.
# The magic number 1 is because the constructor needs an integer so can't
# be NaN. We don't need that field so it can be anything.
nms.append(
NavigationMeasurement(pseudorange, pseudorange, np.nan, dop, dop,
sat_pos, sat_vel, np.nan, np.nan, gpst, sat,
1))
return calc_PVT(nms).pos_ecef
def compute_ecef(pseudoranges, dops, sat_poss, sat_vels, t):
"""
Compute the receiver position if possible, otherwise a vector of NaNs.
Parameters
----------
pseudoranges : Series
Pseudoranges, may have NaNs, but the elements for the keys which are also
keys in dops, must not be NaN.
dops : dict
Dopplers, must not have NaNs.
sat_poss : dict
Satellite positions when this observation was received.
sat_vels : dict
Satellite velocities when this observation was received.
Returns
-------
array
The receiver position in ecef if there's enough info in the input to
compute it, otherwise an array of NaNs.
"""
if len(dops) < 4:
return (np.nan, np.nan, np.nan)
gpst = datetime2gpst(t)
nms = []
for itm in dops.iteritems():
sat = itm[0]
dop = itm[1]
pseudorange = pseudoranges[sat]
sat_pos = sat_poss[sat]
sat_vel = sat_vels[sat]
# TODO, make one of the pseudoranges/dops NaN or actually input and use it,
# instead of using either the corrected/raw for both corrected and raw.
# The magic number 1 is because the constructor needs an integer so can't
# be NaN. We don't need that field so it can be anything.
nms.append(NavigationMeasurement(pseudorange, pseudorange,
np.nan, dop, dop, sat_pos, sat_vel,
np.nan, np.nan, gpst, sat, 1))
return calc_PVT(nms).pos_ecef
def fill_observations(table, cutoff=None, verbose=False):
"""Given a table of observations from a HITL test, fills in some
derived observations using raw base/rover observations and ephemeris
data. Also reconstructs single point positions using these
observations. Returns dicts of each of these quantities, keyed by
GPS timestamp.
Parameters
----------
table : Pandas table
Returns
----------
dict
Derived observations, keyed by the name used for the Panel in the
HDF5 store: {<panel_name> : {<timestamp>: <value>}}
"""
# Holds the navigation measurement from the previous timestep
base_nav_tmp = None
rover_nav_tmp = None
sdiffs = {}
base_spp_sim = {}
rover_spp_sim = {}
i = 0
logging_interval = 1000
start = time.time()
# Iterate through each of the timestamped observations from the rover.
for timestamp, rover_obs_t in table.rover_obs.iteritems():
i += 1
if verbose:
if i % logging_interval == 0:
print "Processed %d records! @ %s sec." % (i, time.time() - start)
if cutoff is not None and i >= int(cutoff):
if verbose:
print "Exiting at %d records! @ %s sec!" % (i, time.time() - start)
break
# Extract obs_base, obs_rover and spp_rover entries for this
# timestep. Filter out invalid ephemerides and observations
eph_t = filter_col(match_ephemeris(timestamp,
table.ephemerides_filled))
base_obs_t = filter_col(match_obs(timestamp, table.base_obs))
if base_obs_t is None or eph_t is None:
continue
# Transform obs_base and obs_rover entries into
# navigation_measurements, which we turn into a set of sdiffs.
base_nav_tmp, base_spp_sim_t \
= derive_gpst_meas(timestamp, base_obs_t, eph_t, base_nav_tmp)
base_spp_sim_t = calc_PVT(base_nav_tmp.values())
rover_nav_tmp, rover_spp_sim_t \
= derive_gpst_meas(timestamp, filter_col(rover_obs_t), eph_t, rover_nav_tmp)
rover_spp_sim_t = calc_PVT(rover_nav_tmp.values())
# Differenced observations
sdiff_ts = mk_single_diff(rover_nav_tmp, base_nav_tmp)
if sdiff_ts:
sdiffs[timestamp] = {t: s.__dict__() for (t, s) in sdiff_ts.iteritems()}
if base_spp_sim_t:
base_spp_sim[timestamp] = dict(zip(['x', 'y', 'z'],
base_spp_sim_t.pos_ecef))
if rover_spp_sim_t:
rover_spp_sim[timestamp] = dict(zip(['x', 'y', 'z'],
rover_spp_sim_t.pos_ecef))
return {'rover_sdiffs': pd.Panel(sdiffs),
'rover_ddiffs': pd.DataFrame({}),
'base_spp_sim': pd.DataFrame(base_spp_sim),
'rover_spp_sim': pd.DataFrame(rover_spp_sim)}
