def compute_gps_satellite_position_from_ephemeris(eph, t, mu=3.986005e14, Omega_e_dot=7.2921151467e-5, compute_velocity=False, rollover=0):
"""Computes and returns the GPS satellite ECEF positions given ephemeris
parameters and a set of times.
parameters:
eph - the ephemeris set (an ordered tuple containing ephemeris information)
`eph` should contain: e, t_oa, i_0, Omega_dot, sqrt_a, Omega_week, omega, M_0, A_f0, A_f1, week TODODODO
t - the desired times for which to compute satellite locations
optional parameters:
mu - Earth gravitational constant (m^3/s^2)
Omega_dot - WGS Earth rotation rate (rad/s)
compute_velocity - flag indicating whether or not to compute satellite velocity as well (default False)
"""
e, t_oe, i_0, Omega_dot, a, Omega_week, omega, M_0, week, delta_n, i_dot, c_us, c_rs, c_is, c_uc, c_rc, c_ic = eph
delta_t = t - gpsseconds(week, t_oe, rollover)
n_0 = sqrt(mu / a**3)
n = n_0 + delta_n # corrected mean motion
M = M_0 + n * delta_t # mean anomaly
E = 0. # eccentric anomaly
err = 1.
tol = 1e-8
while err > tol: # iteratively solve for eccentric anomaly
_temp = M + e * sin(E)
err = npmax(fabs(_temp - E))
E = _temp
nu = arctan2(sqrt(1. - e**2) * sin(E), cos(E) - e) # true anomaly
E = arccos((e + cos(nu)) / (1. + e * cos(nu))) # eccentric anomaly
Phi = nu + omega # argument of latitude
delta_u = c_us * sin(2 * Phi) + c_uc * cos(2 * Phi) # secord-order harmonic perturbation corrections
delta_r = c_rs * sin(2 * Phi) + c_rc * cos(2 * Phi)
delta_i = c_is * sin(2 * Phi) + c_ic * cos(2 * Phi)
u = Phi + delta_u # argument of latitude (corrected)
r = a * (1. - e * cos(E)) + delta_r # orbital radius (corrected)
i = i_0 + delta_i + i_dot * delta_t # inclination (corrected)
Omega = Omega_week + (Omega_dot - Omega_e_dot) * delta_t - Omega_e_dot * t_oe # longitude of ascensing node (corrected)
x_orb, y_orb = r * cos(u), r * sin(u) # compute x,y in orbital plane
x_ecef = x_orb * cos(Omega) - y_orb * sin(Omega) * cos(i) # transform from orbital system to ECEF system
y_ecef = x_orb * sin(Omega) + y_orb * cos(Omega) * cos(i)
z_ecef = y_orb * sin(i) * ones(asarray(Omega).size)
if compute_velocity: # compute x,y velocity in orbital plane
v_x_orb = n * a * sin(E) / (1. - e * cos(E))
v_y_orb = -n * a * sqrt(1. - e**2) * cos(E) / (1. - e * cos(E))
v_x_ecef = v_x_orb * cos(Omega) - v_y_orb * sin(Omega) * cos(i) # transform from orbital system to ECEF system
v_y_ecef = v_x_orb * sin(Omega) + v_y_orb * cos(Omega) * cos(i)
v_z_ecef = v_y_orb * sin(i) * ones(asarray(Omega).size)
return asarray([[x_ecef, y_ecef, z_ecef], [v_x_ecef, v_y_ecef, v_z_ecef]])
else:
return asarray([x_ecef, y_ecef, z_ecef])
def compute_gps_satellite_position_from_yuma(alm, t, mu = 3.986005e14, Omega_e_dot = 7.2921151467e-5, compute_velocity=False, rollover=0):
"""Computes and returns the GPS satellite ECEF positions given a Yuma
almanac and a set of times (seconds into GPS week).
parameters:
alm - the Yuma almanac (an ordered tuple containing Yuma almanac information)
The Yuma almanac should contain: health, e, t_oa, i_0, Omega_dot, sqrt_a, Omega_week, omega, M_0, A_f0, A_f1, week
t - the desired times for which to compute satellite locations
optional parameters:
mu - Earth gravitational constant (m^3/s^2)
Omega_dot - WGS Earth rotation rate (rad/s)
compute_velocity - flag indicating whether or not to compute satellite velocity as well (default False)
"""
health, e, t_oa, i_0, Omega_dot, sqrt_a, Omega_week, omega, M_0, A_f0, A_f1, week = alm
a = sqrt_a**2.
delta_t = t - gpsseconds(week, t_oa, rollover)
n_0 = sqrt(mu / a**3)
n = n_0 # mean motion
M = M_0 + n * delta_t # mean anomaly
E = 0. # eccentric anomaly
err = 1.
tol = 1e-8
while err > tol: # iteratively solve for eccentric anomaly
_temp = M + e * sin(E)
err = npmax(fabs(_temp - E))
E = _temp
nu = arctan2(sqrt(1. - e**2) * sin(E), cos(E) - e) # true anomaly
E = arccos((e + cos(nu)) / (1. + e * cos(nu))) # eccentric anomaly
Phi = nu + omega # argument of latitude
u = Phi # argument of latitude
r = a * (1. - e * cos(E)) # orbital radius
i = i_0 # inclination
Omega = Omega_week + (Omega_dot - Omega_e_dot) * delta_t - Omega_e_dot * t_oa # longitude of ascensing node (corrected)
x_orb, y_orb = r * cos(u), r * sin(u) # compute x,y in orbital plane
x_ecef = x_orb * cos(Omega) - y_orb * sin(Omega) * cos(i) # transform from orbital system to ECEF system
y_ecef = x_orb * sin(Omega) + y_orb * cos(Omega) * cos(i)
z_ecef = y_orb * sin(i) * ones(asarray(Omega).size)
if compute_velocity: # compute x,y velocity in orbital plane
v_x_orb = n * a * sin(E) / (1. - e * cos(E))
v_y_orb = -n * a * sqrt(1. - e**2) * cos(E) / (1. - e * cos(E))
v_x_ecef = v_x_orb * cos(Omega) - v_y_orb * sin(Omega) * cos(i) # transform from orbital system to ECEF system
v_y_ecef = v_x_orb * sin(Omega) + v_y_orb * cos(Omega) * cos(i)
v_z_ecef = v_y_orb * sin(i) * ones(asarray(Omega).size)
return asarray([[x_ecef, y_ecef, z_ecef], [v_x_ecef, v_y_ecef, v_z_ecef]])
else:
return asarray([x_ecef, y_ecef, z_ecef])
def compute_gps_orbital_parameters_from_ephemeris(eph, t, mu=3.986005e14, Omega_e_dot=7.2921151467e-5, rollover=0):
"""Computes and returns the following orbital parameters that define GPS satellite position/velocity:
argument of latitude
orbital radius
orbital inclination
longitude of ascensing node
corrected mean motion
eccentric anomaly
parameters:
eph - the ephemeris set (an ordered tuple containing ephemeris information)
`eph` should contain: e, t_oa, i_0, Omega_dot, sqrt_a, Omega_week, omega, M_0, A_f0, A_f1, week TODODODO
t - the desired times for which to compute satellite locations
optional parameters:
mu - Earth gravitational constant (m^3/s^2)
Omega_dot - WGS Earth rotation rate (rad/s)
rollover - rollover to include when computing GPS time from week number (default 0)
"""
e, t_oe, i_0, a, omega_dot, omega_0, omega, M_0, week, delta_n, i_dot, c_us, c_rs, c_is, c_uc, c_rc, c_ic = \
eph.e, eph.t_oe, eph.i_0, eph.a, eph.omega_dot, eph.omega_0, eph.omega, eph.m_0, eph.week, eph.delta_n, eph.i_dot, \
eph.c_us, eph.c_rs, eph.c_is, eph.c_uc, eph.c_rc, eph.c_ic
delta_t = t - gpsseconds(week, t_oe, rollover)
n_0 = sqrt(mu / a**3)
n = n_0 + delta_n # corrected mean motion
M = M_0 + n * delta_t # mean anomaly
E = 0. # eccentric anomaly
err = 1.
tol = 1e-8
while err > tol: # iteratively solve for eccentric anomaly
_temp = M + e * sin(E)
err = npmax(fabs(_temp - E))
E = _temp
nu = arctan2(sqrt(1. - e**2) * sin(E), cos(E) - e) # true anomaly
E = arccos((e + cos(nu)) / (1. + e * cos(nu))) # eccentric anomaly
Phi = nu + omega # argument of latitude
delta_u = c_us * sin(2 * Phi) + c_uc * cos(2 * Phi) # secord-order harmonic perturbation corrections
delta_r = c_rs * sin(2 * Phi) + c_rc * cos(2 * Phi)
delta_i = c_is * sin(2 * Phi) + c_ic * cos(2 * Phi)
u = Phi + delta_u # argument of latitude (corrected)
r = a * (1. - e * cos(E)) + delta_r # orbital radius (corrected)
i = i_0 + delta_i + i_dot * delta_t # inclination (corrected)
Omega = omega_0 + (omega_dot - Omega_e_dot) * delta_t - Omega_e_dot * t_oe # longitude of ascensing node (corrected)
return u, r, i, Omega, n, E
def read_novatel_ascii(_file, svids=range(1, 33)):
"""
Reads gps log data from file at filepath. File should be NovAtel ASCII
formatted text file. Returns namedtuple structs containing all file data.
`obs, eph, bestpos`
"""
message_sizes = count_novatel_ascii_logs(_file)
POS_SIZE = message_sizes['BESTPOSA']
EPH_SIZE = message_sizes['GPSEPHEMA']
RNG_SIZE = message_sizes['RANGEA']
NUM_FIELDS = 10
obs = {}
for svid in svids:
obs[svid] = Observation()
obs[svid].time = zeros((RNG_SIZE,))
obs[svid].l1 = ChannelObservation()
obs[svid].l2 = ChannelObservation()
obs[svid].l1.adr = zeros((RNG_SIZE,))
obs[svid].l1.dopp= zeros((RNG_SIZE,))
obs[svid].l1.psr = zeros((RNG_SIZE,))
obs[svid].l1.cnr = zeros((RNG_SIZE,))
obs[svid].l2.adr = zeros((RNG_SIZE,))
obs[svid].l2.dopp= zeros((RNG_SIZE,))
obs[svid].l2.psr = zeros((RNG_SIZE,))
obs[svid].time.fill(nan)
obs[svid].l1.adr.fill(nan)
obs[svid].l1.dopp.fill(nan)
obs[svid].l1.psr.fill(nan)
obs[svid].l1.cnr.fill(nan)
obs[svid].l2.adr.fill(nan)
obs[svid].l2.dopp.fill(nan)
obs[svid].l2.psr.fill(nan)
eph = {}
for svid in svids:
eph[svid] = Ephemeris()
bestpos = BestPos()
bestpos.time = zeros((POS_SIZE,))
bestpos.geo = zeros((POS_SIZE, 3))
pos_ind = 0
rng_ind = 0
with open(_file, 'r') as f:
for line in f:
line = line.strip().split(';')
header, data = line[0].split(','), line[1].split('*') # crc after *
data, crc = data[0].split(','), data[1]
week_no, seconds = int(header[5]), float(header[6])
gpstime = gpsseconds(week_no, seconds) # no rollover for NovAtel
if header[0] == '#BESTPOSA':
bestpos.time[pos_ind] = gpstime
bestpos.geo[pos_ind, :] = [float(data[2]), float(data[3]), float(data[4])]
pos_ind += 1
elif header[0] == '#GPSEPHEMA':
prn = uint32( data[0]) # Satellite PRN
eph[prn].prn = prn
eph[prn].tow = float64(data[1]) # time of week
eph[prn].health = uint32( data[2]) # satellite health
eph[prn].iode1 = uint32( data[3]) # issue of ephemeris data
eph[prn].iode2 = uint32( data[4]) # issue of ephemeris data 2
eph[prn].week = uint32( data[5]) # GPS week number
eph[prn].zweek = uint32( data[6]) # GPS zweek number
eph[prn].t_oe = float64(data[7]) # time of applicability for ephemeris (s)
eph[prn].a = float64(data[8]) # semi-major axis (m)
eph[prn].delta_n = float64(data[9]) # mean-motion diff (rad/s)
eph[prn].m_0 = float64(data[10]) # mean anomoly of ref time (rad)
eph[prn].e = float64(data[11]) # eccentricity
eph[prn].omega = float64(data[12]) # argument of perigee
eph[prn].c_uc = float64(data[13]) # argument of latitude (amplitude of cosine, radians)
eph[prn].c_us = float64(data[14]) # argument of latitude (amplitude of sine, radians)
eph[prn].c_rc = float64(data[15]) # orbit radius (amplitude of cosine, meters)
eph[prn].c_rs = float64(data[16]) # orbit radius (amplitude of sine, meters)
eph[prn].c_ic = float64(data[17]) # inclination (amplitude of cosine, meters)
eph[prn].c_is = float64(data[18]) # inclination (amplitude of sine, meters)
eph[prn].i_0 = float64(data[19]) # inclination angle at reference time, (rad)
eph[prn].i_dot = float64(data[20]) # rate of inclination angle, (rad/s)
eph[prn].omega_0 = float64(data[21]) # right ascension at week (rad)
eph[prn].omega_dot = float64(data[22]) # rate of right ascension, (rad/s)
eph[prn].iodc = uint32( data[23]) # issue of data clock
eph[prn].t_oc = float64(data[24]) # SV clock correction term, seconds
eph[prn].t_gd = float64(data[25]) # estimated group delay difference, seconds
eph[prn].a0 = float64(data[26]) # clock aging parameter, seconds (s)
eph[prn].a1 = float64(data[27]) # clock aging parameter, (s/s)
eph[prn].a2 = float64(data[28]) # clock aging parameter, (s/s/s)
eph[prn].n = float64(data[30]) # corrected mean motion, (rad/s)
elif header[0] == '#RANGEA':
num_records = int(data[0])
data = data[1:]
for i in range(0, num_records * NUM_FIELDS, NUM_FIELDS):
# check channel tracking status (NovAtel Manuel p 237)
prn = int(data[i])
obs[prn].time[rng_ind] = gpstime
tracking_status = int(data[i + 9], 16)
if not tracking_status & (0x1 << 21): # L1
obs[prn].l1.psr[rng_ind] = float(data[i + 2])
obs[prn].l1.adr[rng_ind] = float(data[i + 4])
obs[prn].l1.dopp[rng_ind]= float(data[i + 6])
obs[prn].l1.cnr[rng_ind] = float(data[i + 7])
else: # L2
obs[prn].l2.psr[rng_ind] = float(data[i + 2])
obs[prn].l2.dopp[rng_ind]= float(data[i + 6])
obs[prn].l2.adr[rng_ind] = float(data[i + 4])
rng_ind += 1
return obs, eph, bestpos
