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 <math.h>

#include "pvt.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)
{
  return atanf(Q / I) * (float)(1/(2*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 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 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 mean_TOT = 0;

  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];
    mean_TOT += TOTs[i];
    nav_meas[i]->raw_pseudorange_rate = NAV_C * -meas[i]->carrier_freq / GPS_L1_HZ;
    nav_meas[i]->snr = meas[i]->snr;
    nav_meas[i]->prn = meas[i]->prn;
  }

  mean_TOT = mean_TOT/n_channels;

  double clock_err, clock_rate_err;

  for (u8 i=0; i<n_channels; i++) {
    nav_meas[i]->raw_pseudorange = (mean_TOT - TOTs[i])*NAV_C + 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*NAV_C;
    nav_meas[i]->pseudorange_rate = nav_meas[i]->raw_pseudorange_rate \
                                    - clock_rate_err*NAV_C;
  }
}