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)}