track.c

Go to the documentation of this file.

/*
 * Copyright (C) 2012 Swift Navigation Inc.
 * Contact: Fergus Noble <fergus@swift-nav.com>
 *
 * This source is subject to the license found in the file 'LICENSE' which must
 * be be distributed together with this source. All other rights reserved.
 *
 * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
 * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
 */

#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <float.h>

#include "constants.h"
#include "prns.h"
#include "track.h"
#include "ephemeris.h"
#include "tropo.h"
#include "coord_system.h"

void calc_loop_gains(float bw, float zeta, float k, float loop_freq,
                     float *pgain, float *igain)
{
  /* Find the natural frequency. */
  float omega_n = 8.f*bw*zeta / (4.f*zeta*zeta + 1.f);

  /* Some intermmediate values. */
/*
  float T = 1. / loop_freq;
  float denominator = k*(4 + 4*zeta*omega_n*T + omega_n*omega_n*T*T);

  *pgain = 8*zeta*omega_n*T / denominator;
  *igain = 4*omega_n*omega_n*T*T / denominator;
*/
  *igain = omega_n * omega_n / (k * loop_freq);
  *pgain = 2.f * zeta * omega_n / k;
}

float costas_discriminator(float I, float Q)
{
  if (I == 0) {
    // Technically, it should be +/- 0.25, but then we'd have to keep track
    //  of the previous sign do it right, so it's simple enough to just return
    //  the average of 0.25 and -0.25 in the face of that ambiguity, so zero.
    return 0; 
  }
  return atanf(Q / I) * (float)(1/(2*M_PI));
}

float frequency_discriminator(float I, float Q, float prev_I, float prev_Q)
{
  float dot = fabsf(I * prev_I) + fabsf(Q * prev_Q);
  float cross = prev_I * Q - I * prev_Q;
  return atan2f(cross, dot) / ((float) M_PI);
}

float dll_discriminator(correlation_t cs[3])
{
  float early_mag = sqrtf((float)cs[0].I*cs[0].I + (float)cs[0].Q*cs[0].Q);
  float late_mag = sqrtf((float)cs[2].I*cs[2].I + (float)cs[2].Q*cs[2].Q);

  return 0.5f * (early_mag - late_mag) / (early_mag + late_mag);
}

void aided_lf_init(aided_lf_state_t *s, float y0,
                   float pgain, float igain,
                   float aiding_igain)
{
  s->y = y0;
  s->prev_error = 0.f;
  s->pgain = pgain;
  s->igain = igain;
  s->aiding_igain = aiding_igain;
}

float aided_lf_update(aided_lf_state_t *s, float p_i_error, float aiding_error)
{
  s->y += s->pgain * (p_i_error - s->prev_error) + \
          s->igain * p_i_error + \
          s->aiding_igain * aiding_error;
  s->prev_error = p_i_error;

  return s->y;
}

void simple_lf_init(simple_lf_state_t *s, float y0,
                    float pgain, float igain)
{
  s->y = y0;
  s->prev_error = 0.f;
  s->pgain = pgain;
  s->igain = igain;
}

float simple_lf_update(simple_lf_state_t *s, float error)
{
  s->y += s->pgain * (error - s->prev_error) + \
          s->igain * error;
  s->prev_error = error;

  return s->y;
}

void aided_tl_init(aided_tl_state_t *s, float loop_freq,
                   float code_freq, float code_bw,
                   float code_zeta, float code_k,
                   float carr_freq, float carr_bw,
                   float carr_zeta, float carr_k,
                   float carr_freq_igain)
{
  float pgain, igain;

  s->carr_freq = carr_freq;
  s->prev_I = 1.0f; // This works, but is it a really good way to do it?
  s->prev_Q = 0.0f;
  calc_loop_gains(carr_bw, carr_zeta, carr_k, loop_freq, &pgain, &igain);
  aided_lf_init(&(s->carr_filt), carr_freq, pgain, igain, carr_freq_igain);

  calc_loop_gains(code_bw, code_zeta, code_k, loop_freq, &pgain, &igain);
  s->code_freq = code_freq;
  simple_lf_init(&(s->code_filt), code_freq, pgain, igain);
}

void aided_tl_update(aided_tl_state_t *s, correlation_t cs[3])
{
  float carr_error = costas_discriminator(cs[1].I, cs[1].Q);
  float freq_error = frequency_discriminator(cs[1].I, cs[1].Q, s->prev_I, s->prev_Q);
  s->prev_I = cs[1].I;
  s->prev_Q = cs[1].Q;
  s->carr_freq = aided_lf_update(&(s->carr_filt), carr_error, freq_error);

  float code_error = dll_discriminator(cs);
  s->code_freq = simple_lf_update(&(s->code_filt), -code_error); // + s->carr_freq * SCALING_FACTOR
}

void simple_tl_init(simple_tl_state_t *s, float loop_freq,
                    float code_freq, float code_bw,
                    float code_zeta, float code_k,
                    float carr_freq, float carr_bw,
                    float carr_zeta, float carr_k)
{
  float pgain, igain;

  calc_loop_gains(code_bw, code_zeta, code_k, loop_freq, &pgain, &igain);
  s->code_freq = code_freq;
  simple_lf_init(&(s->code_filt), code_freq, pgain, igain);

  calc_loop_gains(carr_bw, carr_zeta, carr_k, loop_freq, &pgain, &igain);
  s->carr_freq = carr_freq;
  simple_lf_init(&(s->carr_filt), carr_freq, pgain, igain);
}

void simple_tl_update(simple_tl_state_t *s, correlation_t cs[3])
{
  float code_error = dll_discriminator(cs);
  s->code_freq = simple_lf_update(&(s->code_filt), -code_error);
  float carr_error = costas_discriminator(cs[1].I, cs[1].Q);
  s->carr_freq = simple_lf_update(&(s->carr_filt), carr_error);
}

void comp_tl_init(comp_tl_state_t *s, float loop_freq,
                  float code_freq, float code_bw,
                  float code_zeta, float code_k,
                  float carr_freq, float carr_bw,
                  float carr_zeta, float carr_k,
                  float tau, float cpc,
                  u32 sched)
{
  float pgain, igain;

  calc_loop_gains(code_bw, code_zeta, code_k, loop_freq, &pgain, &igain);
  s->code_freq = code_freq;
  simple_lf_init(&(s->code_filt), code_freq, pgain, igain);

  calc_loop_gains(carr_bw, carr_zeta, carr_k, loop_freq, &pgain, &igain);
  s->carr_freq = carr_freq;
  simple_lf_init(&(s->carr_filt), carr_freq, pgain, igain);

  s->n = 0;
  s->sched = sched;
  s->carr_to_code = 1.f / cpc;

  s->A = 1.f - (1.f / (loop_freq * tau));
}

void comp_tl_update(comp_tl_state_t *s, correlation_t cs[3])
{
  float carr_error = costas_discriminator(cs[1].I, cs[1].Q);
  s->carr_freq = simple_lf_update(&(s->carr_filt), carr_error);

  float code_error = dll_discriminator(cs);
  s->code_filt.y = 0.f;
  float code_update = simple_lf_update(&(s->code_filt), -code_error);

  if (s->n > s->sched) {
    s->code_freq = s->A * s->code_freq + \
                   s->A * code_update + \
                   (1.f - s->A)*s->carr_to_code*s->carr_freq;
  } else {
    s->code_freq += code_update;
  }

  s->n++;
}


void cn0_est_init(cn0_est_state_t *s, float bw, float cn0_0,
                  float cutoff_freq, float loop_freq)
{
  s->log_bw = 10.f*log10f(bw);
  s->A = cutoff_freq / (loop_freq + cutoff_freq);
  s->I_prev_abs = -1.f;
  s->nsr = powf(10.f, 0.1f*(s->log_bw - cn0_0));
}

float cn0_est(cn0_est_state_t *s, float I)
{
  float P_n, P_s;

  if (s->I_prev_abs < 0.f) {
    /* This is the first iteration, just update the prev state. */
    s->I_prev_abs = fabs(I);
  } else {
    P_n = fabsf(I) - s->I_prev_abs;
    P_n = P_n*P_n;

    P_s = 0.5f*(I*I + s->I_prev_abs*s->I_prev_abs);

    s->I_prev_abs = fabsf(I);

    s->nsr = s->A * (P_n / P_s) + (1.f - s->A) * s->nsr;
  }

  return s->log_bw - 10.f*log10f(s->nsr);
}

void calc_navigation_measurement(u8 n_channels, channel_measurement_t meas[],
                                 navigation_measurement_t nav_meas[], double nav_time,
                                 ephemeris_t ephemerides[])
{
  channel_measurement_t* meas_ptrs[n_channels];
  navigation_measurement_t* nav_meas_ptrs[n_channels];
  ephemeris_t* ephemerides_ptrs[n_channels];

  for (u8 i=0; i<n_channels; i++) {
    meas_ptrs[i] = &meas[i];
    nav_meas_ptrs[i] = &nav_meas[i];
    ephemerides_ptrs[i] = &ephemerides[meas[i].prn];
  }

  calc_navigation_measurement_(n_channels, meas_ptrs, nav_meas_ptrs, nav_time, ephemerides_ptrs);
}

void calc_navigation_measurement_(u8 n_channels, channel_measurement_t* meas[], navigation_measurement_t* nav_meas[], double nav_time, ephemeris_t* ephemerides[])
{
  double TOTs[n_channels];
  double min_TOT = DBL_MAX;

  for (u8 i=0; i<n_channels; i++) {
    TOTs[i] = 1e-3 * meas[i]->time_of_week_ms;
    TOTs[i] += meas[i]->code_phase_chips / 1.023e6;
    TOTs[i] += (nav_time - meas[i]->receiver_time) * meas[i]->code_phase_rate / 1.023e6;

    nav_meas[i]->tot.wn = ephemerides[i]->toe.wn;
    nav_meas[i]->tot.tow = TOTs[i];

    if (gpsdifftime(nav_meas[i]->tot, ephemerides[i]->toe) > 3*24*3600)
      nav_meas[i]->tot.wn -= 1;

    if (TOTs[i] < min_TOT)
      min_TOT = TOTs[i];

    nav_meas[i]->raw_doppler = meas[i]->carrier_freq;
    nav_meas[i]->snr = meas[i]->snr;
    nav_meas[i]->prn = meas[i]->prn;

    nav_meas[i]->carrier_phase = meas[i]->carrier_phase;
    nav_meas[i]->carrier_phase += (nav_time - meas[i]->receiver_time) * meas[i]->carrier_freq;

    nav_meas[i]->lock_counter = meas[i]->lock_counter;
  }

  double clock_err, clock_rate_err;

  for (u8 i=0; i<n_channels; i++) {
    nav_meas[i]->raw_pseudorange = (min_TOT - TOTs[i])*GPS_C + GPS_NOMINAL_RANGE;

    calc_sat_pos(nav_meas[i]->sat_pos, nav_meas[i]->sat_vel, &clock_err, &clock_rate_err, ephemerides[i], nav_meas[i]->tot);

    nav_meas[i]->pseudorange = nav_meas[i]->raw_pseudorange \
                               + clock_err*GPS_C;
    nav_meas[i]->doppler = nav_meas[i]->raw_doppler + clock_rate_err*GPS_L1_HZ;

    nav_meas[i]->tot.tow -= clock_err;
    nav_meas[i]->tot = normalize_gps_time(nav_meas[i]->tot);
  }
}

int nav_meas_cmp(const void *a, const void *b)
{
  return (s8)((navigation_measurement_t*)a)->prn
       - (s8)((navigation_measurement_t*)b)->prn;
}

u8 tdcp_doppler(u8 n_new, navigation_measurement_t *m_new,
                u8 n_old, navigation_measurement_t *m_old,
                navigation_measurement_t *m_corrected)
{
  /* Sort m_new, m_old should already be sorted. */
  qsort(m_new, n_new, sizeof(navigation_measurement_t), nav_meas_cmp);

  u8 i, j, n = 0;

  /* Loop over m_new and m_old and check if a PRN is present in both. */
  for (i=0, j=0; i<n_new && j<n_old; i++, j++) {
    if (m_new[i].prn < m_old[j].prn)
      j--;
    else if (m_new[i].prn > m_old[j].prn)
      i--;
    else {
      /* Copy m_new to m_corrected. */
      memcpy(&m_corrected[n], &m_new[i], sizeof(navigation_measurement_t));
      /* Calculate the Doppler correction between raw and corrected. */
      double dopp_corr = m_corrected[n].doppler - m_corrected[n].raw_doppler;
      /* Calculate raw Doppler from time difference of carrier phase. */
      /* TODO: check that using difference of TOTs here is a valid
       * approximation. */
      m_corrected[n].raw_doppler = (m_new[i].carrier_phase - m_old[j].carrier_phase)
                                    / gpsdifftime(m_new[i].tot, m_old[j].tot);
      /* Re-apply the same correction to the raw Doppler to get the corrected Doppler. */
      m_corrected[n].doppler = m_corrected[n].raw_doppler + dopp_corr;
      n++;
    }
  }

  return n;
}