def pt_step(r_recv, delta_t, ephem, obs_pr, t_recv_ref): # t_recv = t_recv_ref + delta_t residuals = [] los = {} tot = {} wk, tow_ref = datetime_to_tow(t_recv_ref) for prn, ob_pr in obs_pr.iteritems(): range_obs = ob_pr + delta_t * gps.c tof = range_obs / gps.c tot[prn] = tow_ref - tof # Compute predicted range gps_r, gps_v, clock_err, clock_rate_err = calc_sat_pos(ephem[prn], tot[prn], week = wk) gps_r_sagnac = sagnac(gps_r, tof) line_of_sight = gps_r_sagnac - r_recv range_pred = norm(line_of_sight) # Apply GPS satellite clock correction range_pred -= clock_err * gps.c range_residual = range_pred - range_obs residuals.append(range_residual) los[prn] = -line_of_sight / norm(line_of_sight) return residuals, los, tot
def whatsup(ephem, r, t, mask = None): if mask is None: mask = horizon_dip(r) wk, tow = datetime_to_tow(t) satsup = [] for prn in ephem: pos, _, _, _ = ephemeris.calc_sat_pos(ephem[prn], tow, wk, warn_stale = False) az, el = swiftnav.coord_system.wgsecef2azel(pos, r) if ephem[prn]['healthy'] and degrees(el) > mask: satsup.append(prn) return satsup
def whatsdown(ephem, r, t, mask = -45): """ Return sats *below* a certain mask, for sanity check """ wk, tow = datetime_to_tow(t) satsdown = [] for prn in ephem: pos, _, _, _ = ephemeris.calc_sat_pos(ephem[prn], tow, wk, warn_stale = False) az, el = swiftnav.coord_system.wgsecef2azel(pos, r) if degrees(el) < mask: satsdown.append(prn) return satsdown
def whatsup(ephem, r, t, mask=None): if mask is None: mask = horizon_dip(r) wk, tow = datetime_to_tow(t) satsup = [] for prn in ephem: pos, _, _, _ = ephemeris.calc_sat_pos(ephem[prn], tow, wk, warn_stale=False) az, el = swiftnav.coord_system.wgsecef2azel_(pos, r) if ephem[prn]['healthy'] and degrees(el) > mask: satsup.append(prn) return satsup
def whatsdown(ephem, r, t, mask=-45): """ Return sats *below* a certain mask, for sanity check """ wk, tow = datetime_to_tow(t) satsdown = [] for prn in ephem: pos, _, _, _ = ephemeris.calc_sat_pos(ephem[prn], tow, wk, warn_stale=False) az, el = swiftnav.coord_system.wgsecef2azel_(pos, r) if degrees(el) < mask: satsdown.append(prn) return satsdown
def sat_los(tow, pv, ephem): # Iteratively find range to satellite tof = 60e-3 prev_tof = 0 r_recv = pv[0] while np.abs(tof - prev_tof) > 1e-8: gps_r, gps_v, clock_err, clock_rate_err = eph.calc_sat_pos(ephem, tow - tof) gps_r_sagnac = sagnac(gps_r, tof) line_of_sight = gps_r_sagnac - r_recv los_range = np.linalg.norm(line_of_sight) prev_tof = tof tof = los_range / gps.c # TODO: sign of clock error and error rate? x = los_range - clock_err * gps.c # print los_range, x, tof # TODO: rotate satellite velocity by sagnac? v = np.dot(gps_v - pv[1], line_of_sight) / los_range + clock_rate_err * gps.c return x, v
def predict_observables(prior_traj, prior_datetime, prns, ephem, window): from datetime import timedelta from numpy.linalg import norm from numpy import dot """Given a list of PRNs, a set of ephemerides, a nominal capture time (datetime) and a and a time window (seconds), compute the ranges and dopplers for each satellite at 1ms shifts.""" timeres = 50 * gps.code_period # Might be important to keep this an integer number of code periods t0 = prior_datetime - timedelta(seconds=window / 2.0) ranges = {} dopplers = {} for prn in prns: ranges[prn] = [] dopplers[prn] = [] times = [] for tt in np.arange(0, window, timeres): t = t0 + timedelta(seconds = tt) times.append(t) r, v = prior_traj(t) for prn in prns: wk, tow = datetime_to_tow(t) gps_r, gps_v, clock_err, clock_rate_err = calc_sat_pos(ephem[prn], tow, week = wk) # TODO: Should we be applying sagnac correction here? # Compute doppler los_r = gps_r - r ratepred = dot(gps_v - v, los_r) / norm(los_r) shift = (-ratepred / gps.c - clock_rate_err)* gps.l1 # Compute range rangepred = norm(r - gps_r) # Apply GPS satellite clock correction rangepred -= clock_err * gps.c ranges[prn].append(rangepred) dopplers[prn].append(shift) for prn in prns: ranges[prn] = np.array(ranges[prn]) dopplers[prn] = np.array(dopplers[prn]) return ranges, dopplers, times
def vel_solve(r_sol, t_sol, ephem, obs_pseudodopp, los, tot): prns = los.keys() pred_prr = {} for prn in prns: _, gps_v, _, clock_rate_err = calc_sat_pos(ephem[prn], tot[prn]) pred_prr[prn] = -dot(gps_v, los[prn]) + clock_rate_err * gps.c los = np.array(los.values()) obs_prr = -(np.array(obs_pseudodopp.values()) / gps.l1) * gps.c pred_prr = np.array(pred_prr.values()) prr_err = obs_prr - pred_prr G = np.append(los, (np.array([[1] * len(prns)])).transpose(), 1) X = prr_err sol, v_residsq, _, _ = np.linalg.lstsq(G, X) v_sol = sol[0:3] f_sol = (sol[3] / gps.c) * gps.l1 print "Velocity residual norm: %.1f m/s" % math.sqrt(v_residsq) print "Receiver clock frequency error: %+6.1f Hz" % f_sol return v_sol, f_sol