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