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 pred_dopplers(prns, ephem, r, v, t):
wk, tow = datetime_to_tow(t)
dopplers = []
for prn in prns:
gps_r, gps_v, clock_err, clock_rate_err = calc_sat_pos(ephem[prn],
tow, week = wk, warn_stale = False)
los_r = gps_r - r
ratepred = dot(gps_v - v, los_r) / norm(los_r)
dopplers.append((-ratepred / gps.c - clock_rate_err) * gps.l1)
return dopplers
def pred_dopplers(prns, ephem, r, v, t):
wk, tow = datetime_to_tow(t)
dopplers = []
for prn in prns:
gps_r, gps_v, clock_err, clock_rate_err = calc_sat_pos(
ephem[prn], tow, week=wk, warn_stale=False)
los_r = gps_r - r
ratepred = dot(gps_v - v, los_r) / norm(los_r)
dopplers.append((-ratepred / gps.c - clock_rate_err) * gps.l1)
return dopplers
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 interp_traj(traj, t0, t_run, t_skip=0, t_step=0.002, fs=16.368e6, smoothify=False):
tow0 = datetime_to_tow(t0)[1]
t_start = traj[0][0] + t_skip
t_stop = t_start + t_run
step_samps = int(fs * t_step)
step_dt = step_samps/fs
step_samp_ix = np.arange(t_run / step_dt) * step_samps
step_t = t_start + step_samp_ix / fs
step_tow = tow0 + step_t
step_p = map(lambda t: interp_p(traj, t, causal=not smoothify), step_t)
step_v = np.zeros_like(step_p)
step_v[:-1] = np.diff(np.array(step_p), axis=0) / step_dt
step_v[-1] = step_v[-2]
step_pv = np.array(zip(step_p, step_v))
return step_t, step_tow, step_pv, step_samps
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 load_rinex_nav_msg(filename, t, settings):
"""
Import a set of GPS ephemerides from a RINEX 2 or 3 GNSS Navigation Message file
Parameters
----------
filename : string
Path to a RINEX 2 or 3 GNSS Navigation Message File
e.g. from http://igs.bkg.bund.de/root_ftp/NTRIP/BRDC_v3/ or http://qz-vision.jaxa.jp/USE/archives/ephemeris/
t : datetime
The time of your observations. The navigation message files usually
contain multiple ephemerides over a range of epochs; load_rinex3_nav_msg
will return the ephemeris set with epoch closest to t.
Returns
-------
ephem : dict of dicts
dict by 0-indexed PRN of the ephemeris parameters.
e.g. ephem[21]['af0'] contains the first-order clock correction for PRN 22.
"""
f = open(filename, 'r')
ephem = {}
got_header = False
best_depoch = {}
rinex_ver = None
while True:
line = f.readline()[:-1]
if not line:
break
if got_header == False:
if rinex_ver is None:
if line[60:80] != "RINEX VERSION / TYPE":
print line
raise FormatException("Doesn't appear to be a RINEX file")
rinex_ver = int(float(line[0:9]))
# print "Rinex version", rinex_ver
if line[20] != "N":
raise FormatException(
"Doesn't appear to be a Navigation Message file")
if line[60:73] == "END OF HEADER":
got_header = True
continue
if rinex_ver == 3:
if line[0] != 'G':
continue
if rinex_ver == 3:
prn = int(line[1:3]) - 1
epoch = datetime.strptime(line[4:23], "%Y %m %d %H %M %S")
elif rinex_ver == 2:
prn = int(line[0:2]) - 1
epoch = datetime.strptime(line[3:20], "%y %m %d %H %M %S")
line = ' ' + line # Shift 1 char to the right
depoch = abs(epoch - t)
if prn not in best_depoch or depoch < best_depoch[prn]:
line = line.replace('D', 'E') # Handle bizarro float format
best_depoch[prn] = depoch
e = {'epoch': epoch, 'prn': prn}
e['toc'] = datetime_to_tow(epoch)
e['af0'] = float(line[23:42])
e['af1'] = float(line[42:61])
e['af2'] = float(line[61:80])
def read4(f):
line = f.readline()[:-1]
if rinex_ver == 2:
line = ' ' + line # Shift 1 char to the right
line = line.replace('D', 'E') # Handle bizarro float format
return float(line[4:23]), float(line[23:42]),\
float(line[42:61]), float(line[61:80])
e['iode'], e['crs'], e['dn'], e['m0'] = read4(f)
e['cuc'], e['ecc'], e['cus'], e['sqrta'] = read4(f)
toe, e['cic'], e['omega0'], e['cis'] = read4(f)
e['inc'], e['crc'], e['w'], e['omegadot'] = read4(f)
e['inc_dot'], e['l2_codes'], week, e['l2_pflag'] = read4(f)
e['sv_accuracy'], e['health'], e['tgd'], e['iodc'] = read4(f)
f.readline() # Discard last row
e['toe'] = week % 1024, toe # TODO: check mod-1024 situation
e['healthy'] = (e['health'] == 0.0) and depoch < timedelta(
seconds=settings.ephemMaxAge)
ephem[prn] = e
else: # Don't bother with this entry, time is too far off
for l in range(7):
f.readline() # Discard 7 lines
f.close()
count_healthy = sum([1 for e in ephem.values() if e['healthy']])
print "Ephemeris '%s' loaded with %d healthy satellites." % (filename,
count_healthy)
return ephem
def load_rinex_nav_msg(filename, t, settings):
"""
Import a set of GPS ephemerides from a RINEX 2 or 3 GNSS Navigation Message file
Parameters
----------
filename : string
Path to a RINEX 2 or 3 GNSS Navigation Message File
e.g. from http://igs.bkg.bund.de/root_ftp/NTRIP/BRDC_v3/ or http://qz-vision.jaxa.jp/USE/archives/ephemeris/
t : datetime
The time of your observations. The navigation message files usually
contain multiple ephemerides over a range of epochs; load_rinex3_nav_msg
will return the ephemeris set with epoch closest to t.
Returns
-------
ephem : dict of dicts
dict by 0-indexed PRN of the ephemeris parameters.
e.g. ephem[21]['af0'] contains the first-order clock correction for PRN 22.
"""
f = open(filename,'r')
ephem = {}
got_header = False
best_depoch = {}
rinex_ver = None
while True:
line = f.readline()[:-1]
if not line:
break
if got_header == False:
if rinex_ver is None:
if line[60:80] != "RINEX VERSION / TYPE":
print line
raise FormatException("Doesn't appear to be a RINEX file")
rinex_ver = int(float(line[0:9]))
# print "Rinex version", rinex_ver
if line[20] != "N":
raise FormatException("Doesn't appear to be a Navigation Message file")
if line[60:73] == "END OF HEADER":
got_header = True
continue
if rinex_ver == 3:
if line[0] != 'G':
continue
if rinex_ver == 3:
prn = int(line[1:3]) - 1
epoch = datetime.strptime(line[4:23], "%Y %m %d %H %M %S")
elif rinex_ver == 2:
prn = int(line[0:2]) - 1
epoch = datetime.strptime(line[3:20], "%y %m %d %H %M %S")
line = ' ' + line # Shift 1 char to the right
depoch = abs(epoch - t)
if prn not in best_depoch or depoch < best_depoch[prn]:
line = line.replace('D','E') # Handle bizarro float format
best_depoch[prn] = depoch
e = {'epoch': epoch, 'prn': prn}
e['toc'] = datetime_to_tow(epoch)
e['af0'] = float(line[23:42])
e['af1'] = float(line[42:61])
e['af2'] = float(line[61:80])
def read4(f):
line = f.readline()[:-1]
if rinex_ver == 2: line = ' ' + line # Shift 1 char to the right
line = line.replace('D','E') # Handle bizarro float format
return float(line[4:23]), float(line[23:42]),\
float(line[42:61]), float(line[61:80])
e['iode'], e['crs'], e['dn'], e['m0'] = read4(f)
e['cuc'], e['ecc'], e['cus'], e['sqrta'] = read4(f)
toe, e['cic'], e['omega0'], e['cis'] = read4(f)
e['inc'], e['crc'], e['w'], e['omegadot'] = read4(f)
e['inc_dot'], e['l2_codes'], week, e['l2_pflag'] = read4(f)
e['sv_accuracy'], e['health'], e['tgd'], e['iodc'] = read4(f)
f.readline() # Discard last row
e['toe'] = week % 1024, toe # TODO: check mod-1024 situation
e['healthy'] = (e['health'] == 0.0) and depoch < timedelta(
seconds = settings.ephemMaxAge)
ephem[prn] = e
else: # Don't bother with this entry, time is too far off
for l in range(7): f.readline() # Discard 7 lines
f.close()
count_healthy = sum([1 for e in ephem.values() if e['healthy']])
print "Ephemeris '%s' loaded with %d healthy satellites." % (
filename, count_healthy)
return ephem
