track.c
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2012 Swift Navigation Inc.
00003  * Contact: Fergus Noble <fergus@swift-nav.com>
00004  *
00005  * This source is subject to the license found in the file 'LICENSE' which must
00006  * be be distributed together with this source. All other rights reserved.
00007  *
00008  * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
00009  * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
00010  * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
00011  */
00012 
00013 #include <math.h>
00014 
00015 #include "pvt.h"
00016 #include "prns.h"
00017 #include "track.h"
00018 #include "ephemeris.h"
00019 #include "tropo.h"
00020 #include "coord_system.h"
00021 
00100 void calc_loop_gains(float bw, float zeta, float k, float loop_freq,
00101                      float *pgain, float *igain)
00102 {
00103   /* Find the natural frequency. */
00104   float omega_n = 8.f*bw*zeta / (4.f*zeta*zeta + 1.f);
00105 
00106   /* Some intermmediate values. */
00107 /*
00108   float T = 1. / loop_freq;
00109   float denominator = k*(4 + 4*zeta*omega_n*T + omega_n*omega_n*T*T);
00110 
00111   *pgain = 8*zeta*omega_n*T / denominator;
00112   *igain = 4*omega_n*omega_n*T*T / denominator;
00113 */
00114   *igain = omega_n * omega_n / (k * loop_freq);
00115   *pgain = 2.f * zeta * omega_n / k;
00116 }
00117 
00138 float costas_discriminator(float I, float Q)
00139 {
00140   return atanf(Q / I) * (float)(1/(2*M_PI));
00141 }
00142 
00169 float dll_discriminator(correlation_t cs[3])
00170 {
00171   float early_mag = sqrtf((float)cs[0].I*cs[0].I + (float)cs[0].Q*cs[0].Q);
00172   float late_mag = sqrtf((float)cs[2].I*cs[2].I + (float)cs[2].Q*cs[2].Q);
00173 
00174   return 0.5f * (early_mag - late_mag) / (early_mag + late_mag);
00175 }
00176 
00185 void simple_lf_init(simple_lf_state_t *s, float y0,
00186                     float pgain, float igain)
00187 {
00188   s->y = y0;
00189   s->prev_error = 0.f;
00190   s->pgain = pgain;
00191   s->igain = igain;
00192 }
00193 
00210 float simple_lf_update(simple_lf_state_t *s, float error)
00211 {
00212   s->y += s->pgain * (error - s->prev_error) + \
00213           s->igain * error;
00214   s->prev_error = error;
00215 
00216   return s->y;
00217 }
00218 
00233 void simple_tl_init(simple_tl_state_t *s, float loop_freq,
00234                     float code_freq, float code_bw,
00235                     float code_zeta, float code_k,
00236                     float carr_freq, float carr_bw,
00237                     float carr_zeta, float carr_k)
00238 {
00239   float pgain, igain;
00240 
00241   calc_loop_gains(code_bw, code_zeta, code_k, loop_freq, &pgain, &igain);
00242   s->code_freq = code_freq;
00243   simple_lf_init(&(s->code_filt), code_freq, pgain, igain);
00244 
00245   calc_loop_gains(carr_bw, carr_zeta, carr_k, loop_freq, &pgain, &igain);
00246   s->carr_freq = carr_freq;
00247   simple_lf_init(&(s->carr_filt), carr_freq, pgain, igain);
00248 }
00249 
00264 void simple_tl_update(simple_tl_state_t *s, correlation_t cs[3])
00265 {
00266   float code_error = dll_discriminator(cs);
00267   s->code_freq = simple_lf_update(&(s->code_filt), -code_error);
00268   float carr_error = costas_discriminator(cs[1].I, cs[1].Q);
00269   s->carr_freq = simple_lf_update(&(s->carr_filt), carr_error);
00270 }
00271 
00301 void comp_tl_init(comp_tl_state_t *s, float loop_freq,
00302                   float code_freq, float code_bw,
00303                   float code_zeta, float code_k,
00304                   float carr_freq, float carr_bw,
00305                   float carr_zeta, float carr_k,
00306                   float tau, float cpc,
00307                   u32 sched)
00308 {
00309   float pgain, igain;
00310 
00311   calc_loop_gains(code_bw, code_zeta, code_k, loop_freq, &pgain, &igain);
00312   s->code_freq = code_freq;
00313   simple_lf_init(&(s->code_filt), code_freq, pgain, igain);
00314 
00315   calc_loop_gains(carr_bw, carr_zeta, carr_k, loop_freq, &pgain, &igain);
00316   s->carr_freq = carr_freq;
00317   simple_lf_init(&(s->carr_filt), carr_freq, pgain, igain);
00318 
00319   s->n = 0;
00320   s->sched = sched;
00321   s->carr_to_code = 1.f / cpc;
00322 
00323   s->A = 1.f - (1.f / (loop_freq * tau));
00324 }
00325 
00337 void comp_tl_update(comp_tl_state_t *s, correlation_t cs[3])
00338 {
00339   float carr_error = costas_discriminator(cs[1].I, cs[1].Q);
00340   s->carr_freq = simple_lf_update(&(s->carr_filt), carr_error);
00341 
00342   float code_error = dll_discriminator(cs);
00343   s->code_filt.y = 0.f;
00344   float code_update = simple_lf_update(&(s->code_filt), -code_error);
00345 
00346   if (s->n > s->sched) {
00347     s->code_freq = s->A * s->code_freq + \
00348                    s->A * code_update + \
00349                    (1.f - s->A)*s->carr_to_code*s->carr_freq;
00350   } else {
00351     s->code_freq += code_update;
00352   }
00353 
00354   s->n++;
00355 }
00356 
00357 
00370 void cn0_est_init(cn0_est_state_t *s, float bw, float cn0_0,
00371                   float cutoff_freq, float loop_freq)
00372 {
00373   s->log_bw = 10.f*log10f(bw);
00374   s->A = cutoff_freq / (loop_freq + cutoff_freq);
00375   s->I_prev_abs = -1.f;
00376   s->nsr = powf(10.f, 0.1f*(s->log_bw - cn0_0));
00377 }
00378 
00432 float cn0_est(cn0_est_state_t *s, float I)
00433 {
00434   float P_n, P_s;
00435 
00436   if (s->I_prev_abs < 0.f) {
00437     /* This is the first iteration, just update the prev state. */
00438     s->I_prev_abs = fabs(I);
00439   } else {
00440     P_n = fabsf(I) - s->I_prev_abs;
00441     P_n = P_n*P_n;
00442 
00443     P_s = 0.5f*(I*I + s->I_prev_abs*s->I_prev_abs);
00444 
00445     s->I_prev_abs = fabsf(I);
00446 
00447     s->nsr = s->A * (P_n / P_s) + (1.f - s->A) * s->nsr;
00448   }
00449 
00450   return s->log_bw - 10.f*log10f(s->nsr);
00451 }
00452 
00453 void calc_navigation_measurement(u8 n_channels, channel_measurement_t meas[], navigation_measurement_t nav_meas[], double nav_time, ephemeris_t ephemerides[])
00454 {
00455   channel_measurement_t* meas_ptrs[n_channels];
00456   navigation_measurement_t* nav_meas_ptrs[n_channels];
00457   ephemeris_t* ephemerides_ptrs[n_channels];
00458 
00459   for (u8 i=0; i<n_channels; i++) {
00460     meas_ptrs[i] = &meas[i];
00461     nav_meas_ptrs[i] = &nav_meas[i];
00462     ephemerides_ptrs[i] = &ephemerides[meas[i].prn];
00463   }
00464 
00465   calc_navigation_measurement_(n_channels, meas_ptrs, nav_meas_ptrs, nav_time, ephemerides_ptrs);
00466 }
00467 
00468 void calc_navigation_measurement_(u8 n_channels, channel_measurement_t* meas[], navigation_measurement_t* nav_meas[], double nav_time, ephemeris_t* ephemerides[])
00469 {
00470   double TOTs[n_channels];
00471   double mean_TOT = 0;
00472 
00473   for (u8 i=0; i<n_channels; i++) {
00474     TOTs[i] = 1e-3 * meas[i]->time_of_week_ms;
00475     TOTs[i] += meas[i]->code_phase_chips / 1.023e6;
00476     TOTs[i] += (nav_time - meas[i]->receiver_time) * meas[i]->code_phase_rate / 1.023e6;
00477 
00479     nav_meas[i]->tot.wn = ephemerides[i]->toe.wn;
00480     nav_meas[i]->tot.tow = TOTs[i];
00481     mean_TOT += TOTs[i];
00482     nav_meas[i]->raw_pseudorange_rate = NAV_C * -meas[i]->carrier_freq / GPS_L1_HZ;
00483     nav_meas[i]->snr = meas[i]->snr;
00484     nav_meas[i]->prn = meas[i]->prn;
00485   }
00486 
00487   mean_TOT = mean_TOT/n_channels;
00488 
00489   double clock_err, clock_rate_err;
00490 
00491   for (u8 i=0; i<n_channels; i++) {
00492     nav_meas[i]->raw_pseudorange = (mean_TOT - TOTs[i])*NAV_C + NOMINAL_RANGE;
00493 
00494     calc_sat_pos(nav_meas[i]->sat_pos, nav_meas[i]->sat_vel, &clock_err, &clock_rate_err, ephemerides[i], nav_meas[i]->tot);
00495 
00496     nav_meas[i]->pseudorange = nav_meas[i]->raw_pseudorange\
00497                                + clock_err*NAV_C;
00498     nav_meas[i]->pseudorange_rate = nav_meas[i]->raw_pseudorange_rate \
00499                                     - clock_rate_err*NAV_C;
00500   }
00501 }
00502 


enu
Author(s): Mike Purvis
autogenerated on Fri Jan 3 2014 11:21:07