Skip to content

Comparing changes

Choose two branches to see what’s changed or to start a new pull request. If you need to, you can also compare across forks.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also compare across forks.
...
  • 5 commits
  • 2 files changed
  • 0 commit comments
  • 1 contributor
Showing with 293 additions and 15 deletions.
  1. +55 −1 include/track.h
  2. +238 −14 src/track.c
View
56 include/track.h
@@ -32,6 +32,31 @@ typedef struct {
double y; /**< Output variable. */
} simple_lf_state_t;
+/** State structure for a simple tracking loop.
+ * Should be initialised with simple_tl_init().
+ */
+typedef struct {
+ double code_freq; /**< Code phase rate (i.e. frequency). */
+ double carr_freq; /**< Carrier frequency. */
+ simple_lf_state_t code_filt; /**< Code loop filter state. */
+ simple_lf_state_t carr_filt; /**< Carrier loop filter state. */
+} simple_tl_state_t;
+
+/** State structure for a code/carrier phase complimentary filter tracking
+ * loop.
+ * Should be initialised with comp_tl_init().
+ */
+typedef struct {
+ double code_freq; /**< Code phase rate (i.e. frequency). */
+ double carr_freq; /**< Carrier frequency. */
+ simple_lf_state_t code_filt; /**< Code loop filter state. */
+ simple_lf_state_t carr_filt; /**< Carrier loop filter state. */
+ u32 sched; /**< Gain scheduling count. */
+ u32 n; /**< Iteration counter. */
+ double A; /**< Complementary filter crossover gain. */
+} comp_tl_state_t;
+
+
/** \} */
/** Structure representing a complex valued correlation. */
@@ -40,6 +65,16 @@ typedef struct {
double Q; /**< Quadrature correlation. */
} correlation_t;
+/** State structure for the \f$ C / N_0 \f$ estimator.
+ * Should be initialised with cn0_est_init().
+ */
+typedef struct {
+ double log_bw; /**< Noise bandwidth in dBHz. */
+ double A; /**< IIR filter coeff. */
+ double I_prev_abs; /**< Abs. value of the previous in-phase correlation. */
+ double nsr; /**< Noise-to-signal ratio (1 / SNR). */
+} cn0_est_state_t;
+
/** \} */
typedef struct {
@@ -61,7 +96,7 @@ typedef struct {
double sat_vel[3];
} navigation_measurement_t;
-void calc_loop_gains(double bw, double zeta, double k, double sample_freq,
+void calc_loop_gains(double bw, double zeta, double k, double loop_freq,
double *pgain, double *igain);
double costas_discriminator(double I, double Q);
double dll_discriminator(correlation_t cs[3]);
@@ -70,6 +105,25 @@ void simple_lf_init(simple_lf_state_t *s, double y0,
double pgain, double igain);
double simple_lf_update(simple_lf_state_t *s, double error);
+void simple_tl_init(simple_tl_state_t *s, double loop_freq,
+ double code_freq, double code_bw,
+ double code_zeta, double code_k,
+ double carr_freq, double carr_bw,
+ double carr_zeta, double carr_k);
+void simple_tl_update(simple_tl_state_t *s, correlation_t cs[3]);
+
+void comp_tl_init(comp_tl_state_t *s, double loop_freq,
+ double code_freq, double code_bw,
+ double code_zeta, double code_k,
+ double carr_freq, double carr_bw,
+ double carr_zeta, double carr_k,
+ double tau, u32 sched);
+void comp_tl_update(comp_tl_state_t *s, correlation_t cs[3]);
+
+void cn0_est_init(cn0_est_state_t *s, double bw, double cn0_0,
+ double cutoff_freq, double loop_freq);
+double cn0_est(cn0_est_state_t *s, double I);
+
void calc_navigation_measurement(u8 n_channels, channel_measurement_t meas[],
navigation_measurement_t nav_meas[],
double nav_time, ephemeris_t ephemerides[]);
View
252 src/track.c
@@ -68,8 +68,8 @@
* {4 + 4 \zeta \omega_n T + \omega_n^2 T^2}
* \f]
*
- * Where \f$T\f$ is the sampling time and the overall loop gain, \f$k\f$, is
- * the product of the NCO and discriminator gains, \f$ k = K_0 K_d \f$. The
+ * Where \f$T\f$ is the loop update period and the overall loop gain, \f$k\f$,
+ * is the product of the NCO and discriminator gains, \f$ k = K_0 K_d \f$. The
* natural frequency is related to the loop noise bandwidth, \f$B_L\f$ by the
* following relationship:
*
@@ -89,15 +89,15 @@
* \todo This math is all wrong, these slides show the analysis we want:
* http://www.compdsp.com/presentations/Jacobsen/abineau_dpll_analysis.pdf
*
- * \param bw The loop noise bandwidth, \f$B_L\f$.
- * \param zeta The damping ratio, \f$\zeta\f$.
- * \param k The loop gain, \f$k\f$.
- * \param sample_freq The sampling frequency, \f$1/T\f$.
- * \param pgain Where to store the calculated proportional gain,
- * \f$k_p\f$.
- * \param igain Where to store the calculated integral gain, \f$k_i\f$.
+ * \param bw The loop noise bandwidth, \f$B_L\f$.
+ * \param zeta The damping ratio, \f$\zeta\f$.
+ * \param k The loop gain, \f$k\f$.
+ * \param loop_freq The loop update frequency, \f$1/T\f$.
+ * \param pgain Where to store the calculated proportional gain,
+ * \f$k_p\f$.
+ * \param igain Where to store the calculated integral gain, \f$k_i\f$.
*/
-void calc_loop_gains(double bw, double zeta, double k, double sample_freq,
+void calc_loop_gains(double bw, double zeta, double k, double loop_freq,
double *pgain, double *igain)
{
/* Find the natural frequency. */
@@ -105,13 +105,13 @@ void calc_loop_gains(double bw, double zeta, double k, double sample_freq,
/* Some intermmediate values. */
/*
- double T = 1. / sample_freq;
+ double T = 1. / loop_freq;
double 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 * sample_freq);
+ *igain = omega_n * omega_n / (k * loop_freq);
*pgain = 2.0 * zeta * omega_n / k;
}
@@ -162,7 +162,8 @@ double costas_discriminator(double I, double Q)
* -# Understanding GPS: Principles and Applications.
* Elliott D. Kaplan. Artech House, 1996.
*
- * \param cs The prompt in-phase correlation, \f$I_k\f$.
+ * \param cs An array [E, P, L] of correlation_t structs for the Early, Prompt
+ * and Late correlations.
* \return The discriminator value, \f$\varepsilon_k\f$.
*/
double dll_discriminator(correlation_t cs[3])
@@ -215,8 +216,231 @@ double simple_lf_update(simple_lf_state_t *s, double error)
return s->y;
}
+/** Initialise a simple tracking loop.
+ *
+ * For a full description of the loop filter parameters, see calc_loop_gains().
+ *
+ * \param s The tracking loop state struct to initialise.
+ * \param code_freq The initial code phase rate (i.e. frequency).
+ * \param code_bw The code tracking loop noise bandwidth.
+ * \param code_zeta The code tracking loop damping ratio.
+ * \param code_k The code tracking loop gain.
+ * \param carr_freq The initial carrier frequency.
+ * \param carr_bw The carrier tracking loop noise bandwidth.
+ * \param carr_zeta The carrier tracking loop damping ratio.
+ * \param carr_k The carrier tracking loop gain.
+ */
+void simple_tl_init(simple_tl_state_t *s, double loop_freq,
+ double code_freq, double code_bw,
+ double code_zeta, double code_k,
+ double carr_freq, double carr_bw,
+ double carr_zeta, double carr_k)
+{
+ double 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);
+}
+
+/** Update step for the simple tracking loop.
+ *
+ * Implements a basic second-order tracking loop. The code tracking loop is a
+ * second-order DLL using dll_discriminator() as its discriminator function.
+ * The carrier phase tracking loop is a second-order Costas loop using
+ * costas_discriminator().
+ *
+ * The tracking loop output variables, i.e. code and carrier frequencies can be
+ * read out directly from the state struct.
+ *
+ * \param s The tracking loop state struct.
+ * \param cs An array [E, P, L] of correlation_t structs for the Early, Prompt
+ * and Late correlations.
+ */
+void simple_tl_update(simple_tl_state_t *s, correlation_t cs[3])
+{
+ double code_error = dll_discriminator(cs);
+ s->code_freq = simple_lf_update(&(s->code_filt), -code_error);
+ double carr_error = costas_discriminator(cs[1].I, cs[1].Q);
+ s->carr_freq = simple_lf_update(&(s->carr_filt), carr_error);
+}
+
+/** Initialise a code/carrier phase complimentary filter tracking loop.
+ *
+ * For a full description of the loop filter parameters, see calc_loop_gains().
+ *
+ * This filter implements gain scheduling. Before `sched` iterations of the
+ * loop filter the behaviour will be identical to the simple loop filter. After
+ * `sched` iterations, carrier phase information will be used in the code
+ * tracking loop.
+ *
+ * \param s The tracking loop state struct to initialise.
+ * \param code_freq The initial code phase rate (i.e. frequency).
+ * \param code_bw The code tracking loop noise bandwidth.
+ * \param code_zeta The code tracking loop damping ratio.
+ * \param code_k The code tracking loop gain.
+ * \param carr_freq The initial carrier frequency.
+ * \param carr_bw The carrier tracking loop noise bandwidth.
+ * \param carr_zeta The carrier tracking loop damping ratio.
+ * \param carr_k The carrier tracking loop gain.
+ * \param tau The complimentary filter cross-over frequency.
+ * \param sched The gain scheduling count.
+ */
+void comp_tl_init(comp_tl_state_t *s, double loop_freq,
+ double code_freq, double code_bw,
+ double code_zeta, double code_k,
+ double carr_freq, double carr_bw,
+ double carr_zeta, double carr_k,
+ double tau, u32 sched)
+{
+ double 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->A = 1.0 - (1.0 / (loop_freq * tau));
+}
+
+/** Update step for a code/carrier phase complimentary filter tracking loop.
+ *
+ * The tracking loop output variables, i.e. code and carrier frequencies can be
+ * read out directly from the state struct.
+ *
+ * \todo Write proper documentation with math and diagram.
+ *
+ * \param s The tracking loop state struct.
+ * \param cs An array [E, P, L] of correlation_t structs for the Early, Prompt
+ * and Late correlations.
+ */
+void comp_tl_update(comp_tl_state_t *s, correlation_t cs[3])
+{
+ double carr_error = costas_discriminator(cs[1].I, cs[1].Q);
+ s->carr_freq = simple_lf_update(&(s->carr_filt), carr_error);
+
+ double code_error = dll_discriminator(cs);
+ s->code_filt.y = 0;
+ double 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.0 - s->A)*
+ 1.023e6*(1 + (s->carr_freq-4.092e6)/1.57542e9);
+ } else {
+ s->code_freq += code_update;
+ }
+
+ s->n++;
+}
+
+
/** \} */
+/** Initialise the \f$ C / N_0 \f$ estimator state.
+ *
+ * See cn0_est() for a full description.
+ *
+ * \param s The estimator state struct to initialise.
+ * \param bw The loop noise bandwidth in Hz.
+ * \param cn0_0 The initial value of \f$ C / N_0 \f$ in dBHz.
+ * \param cutoff_freq The low-pass filter cutoff frequency, \f$f_c\f$, in Hz.
+ * \param loop_freq The loop update frequency, \f$f\f$, in Hz.
+ */
+void cn0_est_init(cn0_est_state_t *s, double bw, double cn0_0,
+ double cutoff_freq, double loop_freq)
+{
+ s->log_bw = 10*log10(bw);
+ s->A = cutoff_freq / (loop_freq + cutoff_freq);
+ s->I_prev_abs = -1;
+ s->nsr = pow(10, 0.1*(s->log_bw - cn0_0));
+}
+
+/** Estimate the Carrier-to-Noise Density, \f$ C / N_0 \f$ of a tracked signal.
+ *
+ * Implements a modification of the estimator presented in [1]. In [1] the
+ * estimator essentially uses a moving average over the reciprocal of the
+ * Signal-to-Noise Ratio (SNR). To reduce memory utilisation a simple IIR
+ * low-pass filter is used instead.
+ *
+ * The noise and signal powers estimates for the \f$k\f$-th observation,
+ * \f$\hat P_{N, k}\f$ and \f$\hat P_{S, k}\f$, are calculated as follows:
+ *
+ * \f[
+ * \hat P_{N, k} = \left( \left| I_k \right| -
+ * \left| I_{k-1} \right| \right)^2
+ * \f]
+ * \f[
+ * \hat P_{S, k} = \frac{1}{2} \left( I_k^2 + I_{k-1}^2 \right)
+ * \f]
+ *
+ * Where \f$I_k\f$ is the in-phase output of the prompt correlator for the
+ * \f$k\f$-th integration period.
+ *
+ * The "Noise-to-Signal Ratio" (NSR) is estimated and filtered with a simple
+ * low-pass IIR filter:
+ *
+ * \f[
+ * {NSR}_k = A \frac{\hat P_{N, k}}{\hat P_{S, k}} + (1 - A) {NSR}_{k-1}
+ * \f]
+ *
+ * Where the IIR filter coefficient, \f$A\f$ can be calculated in terms of a
+ * cutoff frequency \f$f_c\f$ and the loop update frequency \f$f = 1/T\f$.
+ *
+ * \f[
+ * A = \frac{f_c}{f_c + f}
+ * \f]
+ *
+ * The filtered NSR value is converted to a \f$ C / N_0 \f$ value and returned.
+ *
+ * \f[
+ * \left( \frac{C}{N_0} \right)_k =
+ * 10 \log_{10} \left( \frac{B_{eqn}}{{NSR}_k} \right)
+ * \f]
+ *
+ * References:
+ * -# "Comparison of Four SNR Estimators for QPSK Modulations",
+ * Norman C. Beaulieu, Andrew S. Toms, and David R. Pauluzzi (2000),
+ * IEEE Communications Letters, Vol. 4, No. 2
+ * -# "Are Carrier-to-Noise Algorithms Equivalent in All Situations?"
+ * Inside GNSS, Jan / Feb 2010.
+ *
+ * \param s The estimator state struct to initialise.
+ * \param I The prompt in-phase correlation from the tracking correlators.
+ * \return The Carrier-to-Noise Density, \f$ C / N_0 \f$, in dBHz.
+ */
+double cn0_est(cn0_est_state_t *s, double I)
+{
+ double P_n, P_s;
+
+ if (s->I_prev_abs < 0) {
+ /* This is the first iteration, just update the prev state. */
+ s->I_prev_abs = fabs(I);
+ } else {
+ P_n = fabs(I) - s->I_prev_abs;
+ P_n = P_n*P_n;
+
+ P_s = 0.5*(I*I + s->I_prev_abs*s->I_prev_abs);
+
+ s->I_prev_abs = fabs(I);
+
+ s->nsr = s->A * (P_n / P_s) + (1 - s->A) * s->nsr;
+ }
+
+ return s->log_bw - 10*log10(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];
@@ -226,7 +450,7 @@ void calc_navigation_measurement(u8 n_channels, channel_measurement_t meas[], na
for (u8 i=0; i<n_channels; i++) {
meas_ptrs[i] = &meas[i];
nav_meas_ptrs[i] = &nav_meas[i];
- ephemerides_ptrs[i] = &ephemerides[i];
+ ephemerides_ptrs[i] = &ephemerides[meas[i].prn];
}
calc_navigation_measurement_(n_channels, meas_ptrs, nav_meas_ptrs, nav_time, ephemerides_ptrs);

No commit comments for this range

Something went wrong with that request. Please try again.