Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

Updating discriminator functions and adding simple first order loop f…

…ilter implementation.
  • Loading branch information...
commit 725ed737f8eb05b51bcdede5023f48856fbaf6dd 1 parent f808820
@fnoble authored
Showing with 108 additions and 5 deletions.
  1. +29 −1 include/track.h
  2. +79 −4 src/track.c
View
30 include/track.h
@@ -16,10 +16,32 @@
#include "common.h"
#include "ephemeris.h"
+/** \addtogroup track
+ * \{ */
+
+/** \addtogroup track_loop
+ * \{ */
+
+/** State structure for the simple loop filter.
+ * Should be initialised with simple_lf_init().
+ */
+typedef struct {
+ double pgain; /**< Proportional gain. */
+ double igain; /**< Integral gain. */
+ double prev_error; /**< Previous error. */
+ double y; /**< Output variable. */
+} simple_lf_state_t;
+
+/** \} */
+
+/** Structure representing a complex valued correlation. */
typedef struct {
- double I, Q;
+ double I; /**< In-phase correlation. */
+ double Q; /**< Quadrature correlation. */
} correlation_t;
+/** \} */
+
typedef struct {
u8 prn;
double code_phase_chips;
@@ -41,6 +63,12 @@ typedef struct {
void calc_loop_gains(double bw, double zeta, double k, double sample_freq,
double *pgain, double *igain);
+double costas_discriminator(double I, double Q);
+double dll_discriminator(correlation_t cs[3]);
+
+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 calc_navigation_measurement(u8 n_channels, channel_measurement_t meas[],
navigation_measurement_t nav_meas[],
View
83 src/track.c
@@ -125,19 +125,94 @@ void calc_loop_gains(double bw, double zeta, double k, double sample_freq,
* \varepsilon_k = \tan^{-1} \left(\frac{I_k}{Q_k}\right)
* \f]
*
+ * References:
+ * -# Understanding GPS: Principles and Applications.
+ * Elliott D. Kaplan. Artech House, 1996.
+ *
+ * \todo Fix potential divide by zero if Q is zero.
+ *
* \param I The prompt in-phase correlation, \f$I_k\f$.
* \param Q The prompt quadrature correlation, \f$Q_k\f$.
* \return The discriminator value, \f$\varepsilon_k\f$.
*/
-double costas_discriminator(double I, double Q) {
- return atan(I/Q)/(2*M_PI);
+double costas_discriminator(double I, double Q)
+{
+ return atan(Q / I) / (2*M_PI);
}
-double dll_discriminator(correlation_t cs[3]) {
+/** Normalised non-coherent early-minus-late envelope discriminator.
+ *
+ * Implements the normalised non-coherent early-minus-late envelope DLL
+ * discriminator.
+ *
+ * \f[
+ * \varepsilon_k = \frac{1}{2} \frac{E - L}{E + L}
+ * \f]
+ *
+ * where:
+ *
+ * \f[
+ * E = \sqrt{I^2_E + Q^2_E}
+ * \f]
+ * \f[
+ * L = \sqrt{I^2_L + Q^2_L}
+ * \f]
+ *
+ * References:
+ * -# Understanding GPS: Principles and Applications.
+ * Elliott D. Kaplan. Artech House, 1996.
+ *
+ * \param cs The prompt in-phase correlation, \f$I_k\f$.
+ * \return The discriminator value, \f$\varepsilon_k\f$.
+ */
+double dll_discriminator(correlation_t cs[3])
+{
double early_mag = sqrt((double)cs[0].I*cs[0].I + (double)cs[0].Q*cs[0].Q);
double late_mag = sqrt((double)cs[2].I*cs[2].I + (double)cs[2].Q*cs[2].Q);
- return (early_mag - late_mag) / (early_mag + late_mag);
+ return 0.5 * (early_mag - late_mag) / (early_mag + late_mag);
+}
+
+/** Initialise a simple first-order loop filter.
+ * The gains can be calculated using calc_loop_gains().
+ *
+ * \param s The loop filter state struct to initialise.
+ * \param y0 The initial value of the output variable, \f$y_0\f$.
+ * \param pgain The proportional gain, \f$k_p\f$.
+ * \param igain The integral gain, \f$k_i\f$.
+ */
+void simple_lf_init(simple_lf_state_t *s, double y0,
+ double pgain, double igain)
+{
+ s->y = y0;
+ s->prev_error = 0;
+ s->pgain = pgain;
+ s->igain = igain;
+}
+
+/** Update step for the simple first-order loop filter.
+ *
+ * Implements the first-order loop filter as shown below:
+ *
+ * \image html 1st_order_loop_filter.png Digital loop filter block diagram.
+ *
+ * with transfer function:
+ *
+ * \f[
+ * F[z] = \frac{(k_p+k_i) - k_p z^{-1}}{1 - z^{-1}}
+ * \f]
+ *
+ * \param s The loop filter state struct.
+ * \param error The error output from the discriminator, \f$\varepsilon_k\f$.
+ * \return The updated output variable, \f$y_k\f$.
+ */
+double simple_lf_update(simple_lf_state_t *s, double error)
+{
+ s->y += s->pgain * (error - s->prev_error) + \
+ s->igain * error;
+ s->prev_error = error;
+
+ return s->y;
}
/** \} */
Please sign in to comment.
Something went wrong with that request. Please try again.