I204 l1ca cn0 separation l2c

.gitignore

@@ -9,3 +9,4 @@ clapack-3.2.1-CMAKE/F2CLIBS/libf2c/arith*
 .cproject
 .pydevproject
 .settings/
+.nfs*

include/libswiftnav/track.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2012 Swift Navigation Inc.
+ * Copyright (C) 2012, 2016 Swift Navigation Inc.
  * Contact: Fergus Noble <[email protected]>
  *
  * This source is subject to the license found in the file 'LICENSE' which must
@@ -116,19 +116,44 @@ typedef struct {
 } correlation_t;
 
 /** State structure for the \f$ C / N_0 \f$ estimator.
- * Should be initialised with cn0_est_init().
+ * Should be initialized with cn0_est_init().
  */
 typedef struct {
   float log_bw;     /**< Noise bandwidth in dBHz. */
-  float b;          /**< IIR filter coeff. */
-  float a;          /**< IIR filter coeff. */
   float I_prev_abs; /**< Abs. value of the previous in-phase correlation. */
   float Q_prev_abs; /**< Abs. value of the previous quadrature correlation. */
-  float nsr;        /**< Noise-to-signal ratio (1 / SNR). */
-  float xn;         /**< Last pre-filter sample. */
+  float cn0;        /**< Signal to noise ratio in dB/Hz. */
 } cn0_est_state_t;
 
-/** \} */
+/** State structure for first order low-pass filter.
+ *
+ * \see lp1_filter_init
+ * \see lp1_filter_update
+ */
+typedef struct {
+  float b;          /**< IIR filter coeff. */
+  float a;          /**< IIR filter coeff. */
+  float xn;         /**< Last pre-filter sample. */
+  float yn;         /**< Last post-filter sample. */
+} lp1_filter_t;
+
+/**
+ * Second order Butterworth filter object.
+ *
+ * Structure for filtering CN0 values using 2nd order Butterworth filter.
+ *
+ * \see bw2_filter_init
+ * \see bw2_filter_update
+ */
+typedef struct {
+  float b;          /**< IIR filter coeff. */
+  float a2;         /**< IIR filter coeff. */
+  float a3;         /**< IIR filter coeff. */
+  float yn;         /**< Last post-filter sample. */
+  float yn_prev;    /**< Previous post-filter sample. */
+  float xn;         /**< Last pre-filter sample. */
+  float xn_prev;    /**< Previous pre-filter sample. */
+} bw2_filter_t;
 
 /** This struct holds the state of a tracking channel at a given receiver time epoch.
  *
@@ -172,6 +197,8 @@ typedef struct {
   u16 lock_counter;
 } navigation_measurement_t;
 
+/** \} */
+
 void calc_loop_gains(float bw, float zeta, float k, float loop_freq,
                      float *b0, float *b1);
 float costas_discriminator(float I, float Q);
@@ -228,9 +255,20 @@ void lock_detect_init(lock_detect_t *l, float k1, float k2, u16 lp, u16 lo);
 void lock_detect_reinit(lock_detect_t *l, float k1, float k2, u16 lp, u16 lo);
 void lock_detect_update(lock_detect_t *l, float I, float Q, float DT);
 
-void cn0_est_init(cn0_est_state_t *s, float bw, float cn0_0,
-                  float cutoff_freq, float loop_freq);
-float cn0_est(cn0_est_state_t *s, float I, float Q);
+void cn0_est_bl_init(cn0_est_state_t *s,
+                     float bw, float cn0_0, float f_s, float f_i);
+float cn0_est_bl_update(cn0_est_state_t *s, float I, float Q);
+void cn0_est_snv_init(cn0_est_state_t *s,
+                      float bw, float cn0_0, float f_s, float f_i);
+float cn0_est_snv_update(cn0_est_state_t *s, float I, float Q);
+
+void lp1_filter_init(lp1_filter_t *f, float initial,
+                     float cutoff_freq, float loop_freq);
+float lp1_filter_update(lp1_filter_t *f, float value);
+
+void bw2_filter_init(bw2_filter_t *f, float initial,
+                     float cutoff_freq, float loop_freq);
+float bw2_filter_update(bw2_filter_t *f, float value);
 
 void calc_navigation_measurement(u8 n_channels, const channel_measurement_t *meas[],
                                  navigation_measurement_t *nav_meas[],

python/swiftnav/signal.pxd

@@ -48,16 +48,16 @@ cdef extern from "libswiftnav/signal.h":
   int cmp_sid_sid(const void *a, const void *b)
   bool sid_is_equal(const gnss_signal_t a, const gnss_signal_t b)
 
-  gnss_signal_t construct_sid(code code, u16 sat);
+  gnss_signal_t construct_sid(code code, u16 sat)
   int sid_to_string(char *s, int n, gnss_signal_t sid)
   bool sid_valid(gnss_signal_t sid)
   bool code_valid(code code)
   bool constellation_valid(constellation constellation)
 
-  gnss_signal_t sid_from_code_index(code code, u16 code_index);
-  u16 sid_to_code_index(gnss_signal_t sid);
-  constellation sid_to_constellation(gnss_signal_t sid);
-  constellation code_to_constellation(code code);
+  gnss_signal_t sid_from_code_index(code code, u16 code_index)
+  u16 sid_to_code_index(gnss_signal_t sid)
+  constellation sid_to_constellation(gnss_signal_t sid)
+  constellation code_to_constellation(code code)
 
 cdef class GNSSSignal:
   cdef gnss_signal_t _thisptr

python/swiftnav/track.pxd

@@ -143,18 +143,53 @@ cdef extern from "libswiftnav/track.h":
   void lock_detect_reinit(lock_detect_t *l, float k1, float k2, u16 lp, u16 lo)
   void lock_detect_update(lock_detect_t *l, float I, float Q, float DT)
 
-  # Tracking loop: CN0 est
+  # Tracking loop: CN0 object
   ctypedef struct cn0_est_state_t:
     float log_bw
-    float b
-    float a
     float I_prev_abs
     float Q_prev_abs
-    float nsr
+    float cn0
+
+  void cn0_est_bl_init(cn0_est_state_t *s,
+                       float bw,
+                       float cn0_0,
+                       float f_s,
+                       float f_i)
+  float cn0_est_bl_update(cn0_est_state_t *s, float I, float Q)
+
+  void cn0_est_snv_init(cn0_est_state_t *s,
+                        float bw,
+                        float cn0_0,
+                        float f_s,
+                        float f_i)
+  float cn0_est_snv_update(cn0_est_state_t *s, float I, float Q)
+
+  # Tracking loop: C/N0 filters
+  ctypedef struct lp1_filter_t:
+    float b
+    float a
     float xn
+    float yn
 
-  void cn0_est_init(cn0_est_state_t *s, float bw, float cn0_0, float cutoff_freq, float loop_freq)
-  float cn0_est(cn0_est_state_t *s, float I, float Q)
+  ctypedef struct bw2_filter_t:
+    float b
+    float a2
+    float a3
+    float xn
+    float xn_prev
+    float yn
+    float yn_prev
+
+  void lp1_filter_init(lp1_filter_t *f,
+                       float cn0_0,
+                       float cutoff_freq,
+                       float loop_freq);
+  float lp1_filter_update(lp1_filter_t *f, float cn0);
+  void bw2_filter_init(bw2_filter_t *f,
+                       float cn0_0,
+                       float cutoff_freq,
+                       float loop_freq);
+  float bw2_filter_update(bw2_filter_t *f, float cn0);
 
   # Tracking loop: Navigation measurement
   ctypedef struct channel_measurement_t:
@@ -220,14 +255,24 @@ cdef class LockDetector:
   cdef dict kwargs
   cdef lock_detect_t _thisptr
 
-cdef class CN0Estimator:
-  cdef dict kwargs
+cdef class CN0BLEstimator:
+  cdef cn0_est_state_t _thisptr
+
+cdef class CN0SNVEstimator:
   cdef cn0_est_state_t _thisptr
 
+cdef class LP1Filter:
+  cdef lp1_filter_t _thisptr
+
+cdef class BW2Filter:
+  cdef bw2_filter_t _thisptr
+
 cdef class ChannelMeasurement:
   cdef channel_measurement_t _thisptr
 
 cdef class NavigationMeasurement:
   cdef navigation_measurement_t _thisptr
 
-cdef mk_nav_meas_array(py_nav_meas, u8 n_c_nav_meas, navigation_measurement_t *c_nav_meas)
+cdef mk_nav_meas_array(py_nav_meas,
+                       u8 n_c_nav_meas,
+                       navigation_measurement_t *c_nav_meas)

python/swiftnav/track.pyx

@@ -420,12 +420,7 @@ cdef class AliasDetector:
   def reinit(self, acc_len, time_diff):
     return alias_detect_reinit(&self._thisptr, acc_len, time_diff)
 
-def rebuild_CN0Estimator(kwargs, state):
-    p = CN0Estimator(**kwargs)
-    p._thisptr = state
-    return p
-
-cdef class CN0Estimator:
+cdef class CN0BLEstimator:
   """
   Wraps the `libswiftnav` :math:`C / N_0` estimator implementation.
 
@@ -434,16 +429,16 @@ cdef class CN0Estimator:
 
   """
 
-  def __cinit__(self, **kwargs):
-    self.kwargs = kwargs
-    cn0_est_init(&self._thisptr,
-                 kwargs['bw'],
-                 kwargs['cn0_0'],
-                 kwargs['cutoff_freq'],
-                 kwargs['loop_freq'])
-
-  def __reduce__(self):
-    return (rebuild_CN0Estimator, (self.kwargs, self._thisptr))
+  def __cinit__(self,
+                bw_hz,
+                cn0_0_dbhz,
+                sampleFreq_hz,
+                loopFreq_hz):
+    cn0_est_bl_init(&self._thisptr,
+                    bw_hz,
+                    cn0_0_dbhz,
+                    sampleFreq_hz,
+                    loopFreq_hz)
 
   def update(self, I, Q):
     """
@@ -462,8 +457,72 @@ cdef class CN0Estimator:
       The Carrier-to-Noise Density, :math:`C / N_0`, in dBHz.
 
     """
-    return cn0_est(&self._thisptr, I, Q)
+    return cn0_est_bl_update(&self._thisptr, I, Q)
 
+cdef class CN0SNVEstimator:
+  """
+  Wraps the `libswiftnav` :math:`C / N_0` estimator implementation.
+
+  The estimator state, :libswiftnav:`cn0_est_state_t` is maintained by
+  the class instance.
+
+  """
+
+  def __cinit__(self,
+                bw_hz,
+                cn0_0_dbhz,
+                sampleFreq_hz,
+                loopFreq_hz):
+    cn0_est_snv_init(&self._thisptr,
+                     bw_hz,
+                     cn0_0_dbhz,
+                     sampleFreq_hz,
+                     loopFreq_hz)
+
+  def update(self, I, Q):
+    """
+    Wraps the function :libswiftnav:`cn0_est`.
+
+    Parameters
+    ----------
+    I : float
+      The prompt in-phase correlation from the tracking correlator.
+    Q : float
+      The prompt quadrature correlation from the tracking correlator.
+
+    Returns
+    -------
+    out : float
+      The Carrier-to-Noise Density, :math:`C / N_0`, in dBHz.
+
+    """
+    return cn0_est_snv_update(&self._thisptr, I, Q)
+
+cdef class LP1Filter:
+  def __cinit__(self,
+                cn0_0_dbhz,
+                cutoffFreq_hz,
+                loopFreq_hz):
+    lp1_filter_init(&self._thisptr,
+                    cn0_0_dbhz,
+                    cutoffFreq_hz,
+                    loopFreq_hz)
+
+  def update(self, cn0):
+    return lp1_filter_update(&self._thisptr, cn0)
+
+cdef class BW2Filter:
+  def __cinit__(self,
+                cn0_0_dbhz,
+                cutoffFreq_hz,
+                loopFreq_hz):
+    bw2_filter_init(&self._thisptr,
+                    cn0_0_dbhz,
+                    cutoffFreq_hz,
+                    loopFreq_hz)
+
+  def update(self, cn0):
+    return bw2_filter_update(&self._thisptr, cn0)
 
 cdef class ChannelMeasurement:
 

src/CMakeLists.txt

@@ -47,6 +47,10 @@ set(libswiftnav_SRCS
   bit_sync.c
   cnav_msg.c
   counter_checker/counter_checker.c
+  cn0_est_bl.c
+  cn0_est_snv.c
+  filter_bw2.c
+  filter_lp1.c
   ${plover_SRCS}
 
   CACHE INTERNAL ""