raw_gnss.py

import scipy.optimize as opt
import numpy as np
import datetime
from . import constants
from .lib.coordinates import LocalCoord
from .gps_time import GPSTime
from .helpers import rinex3_obs_from_rinex2_obs, \
                    get_nmea_id_from_prn, \
                    get_prn_from_nmea_id, \
                    get_constellation


def array_from_normal_meas(meas):
  return np.concatenate(([get_nmea_id_from_prn(meas.prn)],
                         [meas.recv_time_week],
                         [meas.recv_time_sec],
                         [meas.glonass_freq],
                         [meas.observables['C1C']],
                         [meas.observables_std['C1C']],
                         [meas.observables['D1C']],
                         [meas.observables_std['D1C']],
                         [meas.observables['S1C']],
                         [meas.observables['L1C']]))


def normal_meas_from_array(arr):
  observables, observables_std = {}, {}
  observables['C1C'] = arr[4]
  observables_std['C1C'] = arr[5]
  observables['D1C'] = arr[6]
  observables_std['D1C'] = arr[7]
  observables['S1C'] = arr[8]
  observables['L1C'] = arr[9]
  return GNSSMeasurement(get_prn_from_nmea_id(arr[0]), arr[1], arr[2],
                         observables, observables_std, arr[3])


class GNSSMeasurement(object):
  PRN = 0
  RECV_TIME_WEEK = 1
  RECV_TIME_SEC = 2
  GLONASS_FREQ = 3

  PR = 4
  PR_STD = 5
  PRR = 6
  PRR_STD = 7

  SAT_POS = slice(8, 11)
  SAT_VEL = slice(11, 14)

  def __init__(self, prn, recv_time_week, recv_time_sec,
               observables, observables_std, glonass_freq=np.nan):

    # Metadata
    self.prn = prn  # satellite ID in rinex convention
    self.recv_time_week = recv_time_week
    self.recv_time_sec = recv_time_sec
    self.recv_time = GPSTime(recv_time_week, recv_time_sec)
    self.glonass_freq = glonass_freq  # glonass channel

    # Measurements
    self.observables = observables
    self.observables_std = observables_std

    # flags
    self.processed = False
    self.corrected = False

    # sat info
    self.sat_pos = np.nan * np.ones(3)
    self.sat_vel = np.nan * np.ones(3)
    self.sat_clock_err = np.nan

    self.sat_pos_final = np.nan * np.ones(3)  # sat_pos in receiver time's ECEF frame instead of satellite time's ECEF frame
    self.observables_final = {}

  def process(self, dog):
    sat_time = self.recv_time - self.observables['C1C']/constants.SPEED_OF_LIGHT
    sat_info = dog.get_sat_info(self.prn, sat_time)
    if sat_info is None:
      return False
    else:
      self.sat_pos = sat_info[0]
      self.sat_vel = sat_info[1]
      self.sat_clock_err = sat_info[2]
      self.processed = True
      return True

  def correct(self, est_pos, dog):
    for obs in self.observables:
      if obs[0] == 'C':  # or obs[0] == 'L':
        delay = dog.get_delay(self.prn, self.recv_time, est_pos, signal=obs)
        if delay:
          self.observables_final[obs] = (self.observables[obs] +
                                         self.sat_clock_err*constants.SPEED_OF_LIGHT -
                                         delay)
      else:
        self.observables_final[obs] = self.observables[obs]
    if 'C1C' in self.observables_final and 'C2P' in self.observables_final:
      self.observables_final['IOF'] = (((constants.GPS_L1**2)*self.observables_final['C1C'] -
                                        (constants.GPS_L2**2)*self.observables_final['C2P'])/
                                       (constants.GPS_L1**2 - constants.GPS_L2**2))

    geometric_range = np.linalg.norm(self.sat_pos - est_pos)
    theta_1 = constants.EARTH_ROTATION_RATE*(geometric_range)/constants.SPEED_OF_LIGHT
    self.sat_pos_final = [self.sat_pos[0]*np.cos(theta_1) + self.sat_pos[1]*np.sin(theta_1),
                          self.sat_pos[1]*np.cos(theta_1) - self.sat_pos[0]*np.sin(theta_1),
                          self.sat_pos[2]]
    if 'C1C' in self.observables_final and np.isfinite(self.observables_final['C1C']):
      self.corrected = True
      return True
    else:
      return False

  def as_array(self):
    if not self.corrected:
      raise NotImplementedError('Only corrected measurements can be put into arrays')
    else:
      ret = np.array([get_nmea_id_from_prn(self.prn), self.recv_time_week, self.recv_time_sec, self.glonass_freq,
                    self.observables_final['C1C'], self.observables_std['C1C'],
                    self.observables_final['D1C'], self.observables_std['D1C']])
      ret = np.concatenate((ret, self.sat_pos_final, self.sat_vel))
      return ret


def process_measurements(measurements, dog=None):
  proc_measurements = []
  for meas in measurements:
    if meas.process(dog):
      proc_measurements.append(meas)
  return proc_measurements


def correct_measurements(measurements, est_pos, dog=None):
  corrected_measurements = []
  for meas in measurements:
    if meas.correct(est_pos, dog):
      corrected_measurements.append(meas)
  return corrected_measurements


def group_measurements_by_epoch(measurements):
  meas_filt_by_t = [[measurements[0]]]
  for m in measurements[1:]:
    if abs(m.recv_time - meas_filt_by_t[-1][-1].recv_time) > 1e-9:
      meas_filt_by_t.append([])
    meas_filt_by_t[-1].append(m)
  return meas_filt_by_t


def group_measurements_by_sat(measurements):
  measurements_by_sat = {}
  sats = set([m.prn for m in measurements])
  for sat in sats:
    measurements_by_sat[sat] = [m for m in measurements if m.prn == sat]
  return measurements_by_sat


def read_raw_qcom(report):
  recv_tow = (report.gpsMilliseconds) * 1.0 / 1000.0  # seconds
  recv_week = report.gpsWeek
  recv_time = GPSTime(recv_week, recv_tow)
  measurements = []
  for i in report.sv:
    svId = i.svId
    if not i.measurementStatus.measurementNotUsable and i.measurementStatus.satelliteTimeIsKnown:
      sat_tow = (
        i.unfilteredMeasurementIntegral + i.unfilteredMeasurementFraction) / 1000
      sat_time = GPSTime(recv_week, sat_tow)
      observables, observables_std = {}, {}
      observables['C1C'] = (recv_time - sat_time)*constants.SPEED_OF_LIGHT
      observables_std['C1C'] = i.unfilteredTimeUncertainty * 1e-3 * constants.SPEED_OF_LIGHT
      observables['D1C'] = i.unfilteredSpeed
      observables_std['D1C'] = i.unfilteredSpeedUncertainty
      observables['S1C'] = np.nan
      observables['L1C'] = np.nan
      measurements.append(GNSSMeasurement(get_prn_from_nmea_id(svId),
                                  recv_time.week,
                                  recv_time.tow,
                                  observables,
                                  observables_std))
  return measurements


def read_raw_ublox(report):
  recv_tow = (report.rcvTow)  # seconds
  recv_week = report.gpsWeek
  recv_time = GPSTime(recv_week, recv_tow)
  measurements = []
  for i in report.measurements:
    # only add gps and glonass fixes
    if (i.gnssId == 0 or i.gnssId==6):
      if i.svId > 32 or i.pseudorange > 2**32:
        continue
      if i.gnssId == 0:
        prn = 'G%02i' % i.svId
      else:
        prn = 'R%02i' % i.svId
      observables = {}
      observables_std = {}
      if i.trackingStatus.pseudorangeValid and i.sigId==0:
        observables['C1C'] = i.pseudorange
        # Empirically it seems obvious ublox's std is
        # actually a variation
        observables_std['C1C'] = np.sqrt(i.pseudorangeStdev)*10
        if i.gnssId==6:
          glonass_freq = i.glonassFrequencyIndex - 7
          observables['D1C'] = -(constants.SPEED_OF_LIGHT / (constants.GLONASS_L1 + glonass_freq*constants.GLONASS_L1_DELTA)) * (i.doppler)
        elif i.gnssId==0:
          glonass_freq = np.nan
          observables['D1C'] = -(constants.SPEED_OF_LIGHT / constants.GPS_L1) * (i.doppler)
        observables_std['D1C'] = (constants.SPEED_OF_LIGHT / constants.GPS_L1) * i.dopplerStdev * 1
        observables['S1C'] = i.cno
        if i.trackingStatus.carrierPhaseValid:
          observables['L1C'] = i.carrierCycles
        else:
          observables['L1C'] = np.nan
        measurements.append(GNSSMeasurement(prn,
                                    recv_time.week,
                                    recv_time.tow,
                                    observables,
                                    observables_std,
                                    glonass_freq))
  return measurements


def read_rinex_obs(obsdata):
  measurements = []
  first_sat = list(obsdata.data.keys())[0]
  n = len(obsdata.data[first_sat]['Epochs'])
  for i in range(0, n):
    recv_time_datetime = obsdata.data[first_sat]['Epochs'][i]
    recv_time_datetime = recv_time_datetime.astype(datetime.datetime)
    recv_time = GPSTime.from_datetime(recv_time_datetime)
    measurements.append([])
    for sat_str in list(obsdata.data.keys()):
      if np.isnan(obsdata.data[sat_str]['C1'][i]):
        continue
      observables, observables_std = {}, {}
      for obs in obsdata.data[sat_str]:
        if obs == 'Epochs':
          continue
        observables[rinex3_obs_from_rinex2_obs(obs)] = obsdata.data[sat_str][obs][i]
        observables_std[rinex3_obs_from_rinex2_obs(obs)] = 1
      measurements[-1].append(GNSSMeasurement(get_prn_from_nmea_id(int(sat_str)),
                              recv_time.week,
                              recv_time.tow,
                              observables,
                              observables_std))
  return measurements


def calc_pos_fix(measurements, x0=[0, 0, 0, 0, 0], no_weight=False, signal='C1C'):
  '''
  Calculates gps fix with WLS optimizer

  returns:
  0 -> list with positions
  1 -> pseudorange errs
  '''
  n = len(measurements)
  if n < 6:
      return []

  Fx_pos = pr_residual(measurements, signal=signal, no_weight=no_weight, no_nans=True)
  opt_pos = opt.least_squares(Fx_pos, x0).x
  return opt_pos, Fx_pos(opt_pos, no_weight=True)


def calc_vel_fix(measurements, est_pos, v0=[0, 0, 0, 0], no_weight=False, signal='D1C'):
  '''
  Calculates gps velocity fix with WLS optimizer

  returns:
  0 -> list with velocities
  1 -> pseudorange_rate errs
  '''
  n = len(measurements)
  if n < 6:
      return []

  Fx_vel = prr_residual(measurements, est_pos, no_weight=no_weight, no_nans=True)
  opt_vel = opt.least_squares(Fx_vel, v0).x
  return opt_vel, Fx_vel(opt_vel, no_weight=True)


def pr_residual(measurements, signal='C1C', no_weight=False, no_nans=False):
  # solve for pos
  def Fx_pos(xxx_todo_changeme, no_weight=no_weight):
    (x, y, z, bc, bg) = xxx_todo_changeme
    rows = []

    for meas in measurements:
      if signal in meas.observables_final and np.isfinite(meas.observables_final[signal]):
        pr = meas.observables_final[signal]
        sat_pos = meas.sat_pos_final
        theta = 0
      elif signal in meas.observables and np.isfinite(meas.observables[signal]) and meas.processed:
        pr = meas.observables[signal]
        pr += meas.sat_clock_err * constants.SPEED_OF_LIGHT
        sat_pos = meas.sat_pos
        theta = constants.EARTH_ROTATION_RATE * (pr - bc) / constants.SPEED_OF_LIGHT
      else:
        if not no_nans:
          rows.append(np.nan)
        continue
      if no_weight:
        weight = 1
      else:
        weight = (1 / meas.observables_std[signal])

      if get_constellation(meas.prn) == 'GLONASS':
        rows.append(weight * (np.sqrt(
          (sat_pos[0] * np.cos(theta) + sat_pos[1] * np.sin(theta) - x)**2 +
          (sat_pos[1] * np.cos(theta) - sat_pos[0] * np.sin(theta) - y)**2 +
          (sat_pos[2] - z)**2) - (pr - bc - bg)))
      elif get_constellation(meas.prn) == 'GPS':
        rows.append(weight * (np.sqrt(
          (sat_pos[0] * np.cos(theta) + sat_pos[1] * np.sin(theta) - x)**2 +
          (sat_pos[1] * np.cos(theta) - sat_pos[0] * np.sin(theta) - y)**2 +
          (sat_pos[2] - z)**2) - (pr - bc)))
    return rows
  return Fx_pos


def prr_residual(measurements, est_pos, signal='D1C', no_weight=False, no_nans=False):
  # solve for vel
  def Fx_vel(vel, no_weight=no_weight):
    rows = []
    for meas in measurements:
      if signal not in meas.observables or not np.isfinite(meas.observables[signal]):
        if not no_nans:
          rows.append(np.nan)
        continue
      if meas.corrected:
        sat_pos = meas.sat_pos_final
      else:
        sat_pos = meas.sat_pos
      if no_weight:
        weight = 1
      else:
        weight = (1 / meas.observables[signal])
      los_vector = (sat_pos - est_pos[0:3]
                    ) / np.linalg.norm(sat_pos - est_pos[0:3])
      rows.append(
        weight * ((meas.sat_vel - vel[0:3]).dot(los_vector) -
                  (meas.observables[signal] - vel[3])))
    return rows
  return Fx_vel


def get_Q(recv_pos, sat_positions):
  local = LocalCoord.from_ecef(recv_pos)
  sat_positions_rel = local.ecef2ned(sat_positions)
  sat_distances = np.linalg.norm(sat_positions_rel, axis=1)
  A = np.column_stack((sat_positions_rel[:,0]/sat_distances,  # pylint: disable=unsubscriptable-object
                       sat_positions_rel[:,1]/sat_distances,  # pylint: disable=unsubscriptable-object
                       sat_positions_rel[:,2]/sat_distances,  # pylint: disable=unsubscriptable-object
                       -np.ones(len(sat_distances))))
  if A.shape[0] < 4 or np.linalg.matrix_rank(A) < 4:
    return np.inf*np.ones((4,4))
  else:
    Q = np.linalg.inv(A.T.dot(A))
    return Q


def get_DOP(recv_pos, sat_positions):
  Q = get_Q(recv_pos, sat_positions)
  return np.sqrt(np.trace(Q))


def get_HDOP(recv_pos, sat_positions):
  Q = get_Q(recv_pos, sat_positions)
  return np.sqrt(np.trace(Q[:2,:2]))


def get_VDOP(recv_pos, sat_positions):
  Q = get_Q(recv_pos, sat_positions)
  return np.sqrt(Q[2,2])


def get_TDOP(recv_pos, sat_positions):
  Q = get_Q(recv_pos, sat_positions)
  return np.sqrt(Q[3,3])


def get_PDOP(recv_pos, sat_positions):
  Q = get_Q(recv_pos, sat_positions)
  return np.sqrt(np.trace(Q[:3,:3]))