From dcd4f235b175e8a35bcc113807e6aa3a4f620fe3 Mon Sep 17 00:00:00 2001 From: Tommi Paakki Date: Thu, 7 Apr 2016 11:45:17 +0300 Subject: [PATCH 1/2] CN0 filter redesign + review comment fixes --- include/libswiftnav/track.h | 5 ++- python/swiftnav/track.pxd | 5 ++- src/track.c | 44 ++++++++++++++------ tests/CMakeLists.txt | 1 + tests/check_cn0_estimator.c | 82 +++++++++++++++++++++++++++++++++++++ tests/check_main.c | 1 + tests/check_suites.h | 1 + 7 files changed, 125 insertions(+), 14 deletions(-) create mode 100644 tests/check_cn0_estimator.c diff --git a/include/libswiftnav/track.h b/include/libswiftnav/track.h index 8c5a9e26..27cf88dc 100644 --- a/include/libswiftnav/track.h +++ b/include/libswiftnav/track.h @@ -121,11 +121,14 @@ typedef struct { typedef struct { float log_bw; /**< Noise bandwidth in dBHz. */ float b; /**< IIR filter coeff. */ - float a; /**< IIR filter coeff. */ + float a2; /**< IIR filter coeff. */ + float a3; /**< 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 nsr_prev; /**< Previous Noise-to-signal ratio. */ float xn; /**< Last pre-filter sample. */ + float xn_prev; /**< Previous pre-filter sample. */ } cn0_est_state_t; /** \} */ diff --git a/python/swiftnav/track.pxd b/python/swiftnav/track.pxd index 1eb78093..c3bae14a 100644 --- a/python/swiftnav/track.pxd +++ b/python/swiftnav/track.pxd @@ -147,11 +147,14 @@ cdef extern from "libswiftnav/track.h": ctypedef struct cn0_est_state_t: float log_bw float b - float a + float a2 + float a3 float I_prev_abs float Q_prev_abs float nsr + float nsr_prev float xn + float xn_prev 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) diff --git a/src/track.c b/src/track.c index d29c7121..9a0f32b8 100644 --- a/src/track.c +++ b/src/track.c @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -672,14 +673,22 @@ 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 Tw0 = (2*M_PI*cutoff_freq) / loop_freq; - s->b = Tw0 / (Tw0 + 2); - s->a = (Tw0 - 2) / (Tw0 + 2); + assert(loop_freq != 0.0f); + float Tw0 = tanf((M_PI*cutoff_freq) / loop_freq); + float tmp = 1.0+sqrtf(2)*Tw0+Tw0*Tw0; + assert(tmp != 0.0f); + + s->b = Tw0*Tw0/tmp; + s->a2 = (-2.0+2.0*Tw0*Tw0)/tmp; + s->a3 = (1.0-sqrtf(2)*Tw0+Tw0*Tw0)/tmp; s->log_bw = 10.f*log10f(bw); s->I_prev_abs = -1.f; s->Q_prev_abs = -1.f; s->nsr = powf(10.f, 0.1f*(s->log_bw - cn0_0)); + s->nsr_prev = s->nsr; + s->xn = s->nsr; + s->xn_prev = s->nsr; } /** Estimate the Carrier-to-Noise Density, \f$ C / N_0 \f$ of a tracked signal. @@ -693,8 +702,8 @@ void cn0_est_init(cn0_est_state_t *s, float bw, float cn0_0, * \f$\hat P_{N, k}\f$ and \f$\hat P_{S, k}\f$, are calculated as follows: * * \f[ - * \hat P_{N, k} = \left( \left| Q_k \right| - - * \left| Q_{k-1} \right| \right)^2 + * \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) @@ -703,18 +712,20 @@ void cn0_est_init(cn0_est_state_t *s, float bw, float cn0_0, * 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 first + * The "Noise-to-Signal Ratio" (NSR) is estimated and filtered with a second * order low-pass IIR filter with transfer function: * * \f[ - * F(s) = \frac{\omega_0}{s + \omega_0} + * F(s) = \frac{\omega_0^2}{s^2 + s\sqrt{2}\omega_0 + \omega_0^2} * \f] * The bilinear transform is applied to obtain a digital equivalent * \f[ - * F(z) = \frac{b + bz^{-1}}{1 + az^{-1}} + * F(z) = \frac{b + 2bz^{-1} + bz^{-2}}{1 + a_2z^{-1} + a_3z^{-2}} * \f] - * where \f$ b = \frac{T\omega_0}{T\omega_0 + 2} \f$ - * and \f$ a = \frac{T\omega_0 - 2}{T\omega_0 + 2} \f$. + * where \f$ \omega_c = \frac{2}{T}\tan (2\pi f_{cut}\frac{T}{2}) \f$ + * and \f$ b = \frac{\omega_c^2}{1 + \sqrt{2}\omega_c + \omega_c^2} \f$ + * and \f$ a_2 = \frac{-2 + 2\omega_c^2}{1 + \sqrt{2}\omega_c + \omega_c^2} \f$ + * and \f$ a_3 = \frac{1 - \sqrt{2}\omega_c + \omega_c^2}{1 + \sqrt{2}\omega_c + \omega_c^2} \f$. * * The filtered NSR value is converted to a \f$ C / N_0 \f$ value and returned. * @@ -752,8 +763,17 @@ float cn0_est(cn0_est_state_t *s, float I, float Q) s->I_prev_abs = fabsf(I); s->Q_prev_abs = fabsf(Q); - float tmp = s->b * P_n / P_s; - s->nsr = tmp + s->xn - s->a * s->nsr; + /* This is to avoid division by zero. */ + if (P_s == 0.0) { + return s->log_bw - 10.f*log10f(s->nsr); + } + + float tmp = P_n / P_s; + float tmp2 = s->nsr; + s->nsr = tmp * s->b + s->xn * 2*s->b + s->xn_prev * s->b \ + - s->a2 * s->nsr - s->a3 * s->nsr_prev; + s->nsr_prev = tmp2; + s->xn_prev = s->xn; s->xn = tmp; } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index f1d8718a..f4a3abd9 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -49,6 +49,7 @@ else (CMAKE_CROSSCOMPILING) check_track.c check_cnav.c check_counter_checker.c + check_cn0_estimator.c ) target_link_libraries(test_libswiftnav ${TEST_LIBS}) diff --git a/tests/check_cn0_estimator.c b/tests/check_cn0_estimator.c new file mode 100644 index 00000000..9e159181 --- /dev/null +++ b/tests/check_cn0_estimator.c @@ -0,0 +1,82 @@ +#include +#include +#include +#include +#include + +#define BW 1000 +#define CN0_0 40 +#define CUTOFF_FREQ 0.1 +#define LOOP_FREQ 1000 + +static s8* generate_I_input(u32 length) +{ + s8* input; + u32 ii = 0; + + input = malloc(length); + if (NULL == input) { + return NULL; + } + + for (ii = 0; ii < length; ii++) { + input[ii] = 100; + } + + return input; +} + +static s8* generate_Q_input(u32 length) +{ + s8* input; + u32 ii = 0; + + input = malloc(length); + if (NULL == input) { + return NULL; + } + + for (ii = 0; ii < length; ii++) { + input[ii] = 50; + } + + return input; +} + +START_TEST(test_cn0) +{ + cn0_est_state_t s; + s8* signal_I; + s8* signal_Q; + u32 ii = 0; + u32 test_length = 1000; + float cn0 = 0.0; + + signal_I = generate_I_input(test_length); + fail_if(NULL == signal_I, "Could not allocate I data"); + signal_Q = generate_Q_input(test_length); + fail_if(NULL == signal_Q, "Could not allocate Q data"); + + cn0_est_init(&s, BW, CN0_0, CUTOFF_FREQ, LOOP_FREQ); + + for(ii = 0; ii < test_length; ii++) { + cn0 = cn0_est(&s, signal_I[ii], signal_Q[ii]); + } + + fail_if(cn0 < 30.0); + + free(signal_I); + free(signal_Q); +} +END_TEST + +Suite* cn0_suite(void) +{ + Suite *s = suite_create("CN0"); + TCase *tc_core = tcase_create("Core"); + + tcase_add_test(tc_core, test_cn0); + suite_add_tcase(s, tc_core); + + return s; +} diff --git a/tests/check_main.c b/tests/check_main.c index 6603318d..128de18b 100644 --- a/tests/check_main.c +++ b/tests/check_main.c @@ -35,6 +35,7 @@ int main(void) srunner_add_suite(sr, cnav_test_suite()); srunner_add_suite(sr, correlator_suite()); srunner_add_suite(sr, counter_checker_suite()); + srunner_add_suite(sr, cn0_suite()); srunner_set_fork_status(sr, CK_NOFORK); srunner_run_all(sr, CK_NORMAL); diff --git a/tests/check_suites.h b/tests/check_suites.h index 3994ba1c..1ee9cfd3 100644 --- a/tests/check_suites.h +++ b/tests/check_suites.h @@ -25,5 +25,6 @@ Suite* track_test_suite(void); Suite* cnav_test_suite(void); Suite* correlator_suite(void); Suite* counter_checker_suite(void); +Suite* cn0_suite(void); #endif /* CHECK_SUITES_H */ From 82660b368528fd35688667eded52953aa63c04d9 Mon Sep 17 00:00:00 2001 From: Valeri Atamaniouk Date: Mon, 2 May 2016 14:49:04 +0300 Subject: [PATCH 2/2] cn0: separated C/N0 computation and filtering Separated C/NO computation and filtering algorithms. Updated unit tests and Python bindings. Added SNV CN0 and first order LPF. --- .gitignore | 1 + include/libswiftnav/track.h | 59 +++++++++++--- python/swiftnav/signal.pxd | 10 +-- python/swiftnav/track.pxd | 64 ++++++++++++--- python/swiftnav/track.pyx | 93 ++++++++++++++++++---- src/CMakeLists.txt | 4 + src/cn0_est_bl.c | 112 ++++++++++++++++++++++++++ src/cn0_est_snv.c | 114 ++++++++++++++++++++++++++ src/filter_bw2.c | 95 ++++++++++++++++++++++ src/filter_lp1.c | 93 ++++++++++++++++++++++ src/track.c | 120 ---------------------------- tests/CMakeLists.txt | 1 + tests/check_cn0_estimator.c | 66 ++++++++++++++- tests/check_cn0_filters.c | 155 ++++++++++++++++++++++++++++++++++++ tests/check_main.c | 1 + tests/check_suites.h | 1 + 16 files changed, 820 insertions(+), 169 deletions(-) create mode 100644 src/cn0_est_bl.c create mode 100644 src/cn0_est_snv.c create mode 100644 src/filter_bw2.c create mode 100644 src/filter_lp1.c create mode 100644 tests/check_cn0_filters.c diff --git a/.gitignore b/.gitignore index ecc9600a..d9801ce3 100644 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,4 @@ clapack-3.2.1-CMAKE/F2CLIBS/libf2c/arith* .cproject .pydevproject .settings/ +.nfs* diff --git a/include/libswiftnav/track.h b/include/libswiftnav/track.h index 27cf88dc..64dcb4af 100644 --- a/include/libswiftnav/track.h +++ b/include/libswiftnav/track.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Swift Navigation Inc. + * Copyright (C) 2012, 2016 Swift Navigation Inc. * Contact: Fergus Noble * * This source is subject to the license found in the file 'LICENSE' which must @@ -116,22 +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 I_prev_abs; /**< Abs. value of the previous in-phase correlation. */ + float Q_prev_abs; /**< Abs. value of the previous quadrature correlation. */ + 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 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 nsr_prev; /**< Previous Noise-to-signal ratio. */ + 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. */ -} cn0_est_state_t; - -/** \} */ +} bw2_filter_t; /** This struct holds the state of a tracking channel at a given receiver time epoch. * @@ -175,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); @@ -231,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[], diff --git a/python/swiftnav/signal.pxd b/python/swiftnav/signal.pxd index 4ca96902..eac5fd0f 100644 --- a/python/swiftnav/signal.pxd +++ b/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 diff --git a/python/swiftnav/track.pxd b/python/swiftnav/track.pxd index c3bae14a..848c77d8 100644 --- a/python/swiftnav/track.pxd +++ b/python/swiftnav/track.pxd @@ -143,21 +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 I_prev_abs + float Q_prev_abs + 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 + + ctypedef struct bw2_filter_t: float b float a2 float a3 - float I_prev_abs - float Q_prev_abs - float nsr - float nsr_prev float xn float xn_prev - - 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) + 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: @@ -223,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) diff --git a/python/swiftnav/track.pyx b/python/swiftnav/track.pyx index 9c4476f6..cc3b5278 100644 --- a/python/swiftnav/track.pyx +++ b/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: diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 67348bef..04debbbb 100644 --- a/src/CMakeLists.txt +++ b/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 "" diff --git a/src/cn0_est_bl.c b/src/cn0_est_bl.c new file mode 100644 index 00000000..f67d6f60 --- /dev/null +++ b/src/cn0_est_bl.c @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Valeri Atamaniouk + * + * 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 + +#include +#include + +/** \defgroup track Tracking + * Functions used in tracking. + * \{ */ + +/** Multiplier for checking out-of bounds NSR */ +#define CN0_BL_NSR_MIN_MULTIPLIER (1e-6f) +/** Maximum supported NSR value (1/NSR_MIN_MULTIPLIER)*/ +#define CN0_BL_NSR_MIN (1e6f) + +/** Initialize the \f$ C / N_0 \f$ estimator state. + * + * Initializes Beaulieu's method \f$ C / N_0 \f$ estimator. + * + * The method uses the function for SNR computation: + * + * \f[ + * \frac{C}{N_0}(n) = \frac{1}{2}(\frac{P_n(n)}{P_s(n)}) + * \f] + * where + * \f[ + * P_n(n) = (|Q(n)|-|Q(n-1)|)^2 + * \f] + * \f[ + * P_s(n) = \frac{1}{2}(I(n)^2 + I(n-1)^2) + * \f] + * + * \param s The estimator state struct to initialize. + * \param bw The loop noise bandwidth in Hz. + * \param cn0_0 The initial value of \f$ C / N_0 \f$ in dBHz. + * \param f_s Input sampling frequency in Hz. + * \param f_i Loop integration frequency in Hz. + * + * \return None + */ +void cn0_est_bl_init(cn0_est_state_t *s, + float bw, + float cn0_0, + float f_s, + float f_i) +{ + memset(s, 0, sizeof(*s)); + + /* Normalize by sampling frequency and integration period */ + s->log_bw = 10.f * log10f(bw * f_i / f_s); + s->I_prev_abs = -1.f; + s->Q_prev_abs = -1.f; + s->cn0 = cn0_0; +} + +/** + * Computes \f$ C / N_0 \f$ with Beaulieu's method. + * + * \param s Initialized estimator object. + * \param I In-phase signal component + * \param Q Quadrature phase signal component. + * + * \return Computed \f$ C / N_0 \f$ value + */ +float cn0_est_bl_update(cn0_est_state_t *s, float I, float Q) +{ + /* Compute values for this iteration */ + float I_abs = fabsf(I); + float Q_abs = fabsf(Q); + float I_prev_abs = s->I_prev_abs; + float Q_prev_abs = s->Q_prev_abs; + s->I_prev_abs = I_abs; + s->Q_prev_abs = Q_abs; + + if (I_prev_abs < 0.f) { + /* This is the first iteration, just update the prev state. */ + } else { + float P_n; /* Noise power */ + float P_s; /* Signal power */ + float nsr; /* Noise to signal ratio */ + float nsr_db; /* Noise to signal ratio in dB*/ + + P_n = Q_abs - Q_prev_abs; + P_n *= P_n; + + P_s = 0.5f * (I * I + I_prev_abs * I_prev_abs); + + /* Ensure the NSR is within the limit */ + if (P_s < P_n * CN0_BL_NSR_MIN_MULTIPLIER) + nsr = CN0_BL_NSR_MIN; + else + nsr = P_n / P_s; + + nsr_db = 10.f * log10f(nsr); + s->cn0 = s->log_bw - nsr_db; + } + + return s->cn0; +} + +/** \} */ diff --git a/src/cn0_est_snv.c b/src/cn0_est_snv.c new file mode 100644 index 00000000..869eb789 --- /dev/null +++ b/src/cn0_est_snv.c @@ -0,0 +1,114 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Valeri Atamaniouk + * + * 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 + +#include +#include + +/** \defgroup track Tracking + * Functions used in tracking. + * \{ */ + +/** Multiplier for checking out-of bounds NSR */ +#define CN0_SNV_NSR_MIN_MULTIPLIER (1e-6f) +/** Maximum supported NSR value (1/NSR_MIN_MULTIPLIER)*/ +#define CN0_SNV_NSR_MIN (1e6f) + +/** Initialize the \f$ C / N_0 \f$ estimator state. + * + * Initializes Signal-to-Noise Variance method \f$ C / N_0 \f$ estimator. + * + * The method uses the function for SNR computation: + * + * \f[ + * \frac{C}{N_0}(n) = \frac{P_d}{P_tot-P_d} + * \f] + * where + * \f[ + * P_d(n) = (\frac{1}{2}(|I(n)|+|I(n-1)|))^2 + * \f] + * \f[ + * P_tot(n) = \frac{1}{2}(I(n)^2 + I(n-1)^2 + Q(n)^2 + Q(n-1)^2) + * \f] + * + * \param s The estimator state struct to initialize. + * \param bw The loop noise bandwidth in Hz. + * \param cn0_0 The initial value of \f$ C / N_0 \f$ in dBHz. + * \param f_s Input sampling frequency in Hz. + * \param f_i Loop integration frequency in Hz. + * + * \return None + */ +void cn0_est_snv_init(cn0_est_state_t *s, + float bw, + float cn0_0, + float f_s, + float f_i) +{ + memset(s, 0, sizeof(*s)); + + /* Normalize by sampling frequency and integration period */ + s->log_bw = 10.f*log10f(bw * f_i / f_s); + s->I_prev_abs = -1.f; + s->Q_prev_abs = -1.f; + s->cn0 = cn0_0; +} + +/** + * Computes \f$ C / N_0 \f$ with Signal-to-Noise Variance method. + * + * \param s Initialized estimator object. + * \param I In-phase signal component + * \param Q Quadrature phase signal component. + * + * \return Computed \f$ C / N_0 \f$ value + */ +float cn0_est_snv_update(cn0_est_state_t *s, float I, float Q) +{ + float I_abs = fabsf(I); + float Q_abs = fabsf(Q); + float I_prev_abs = s->I_prev_abs; + float Q_prev_abs = s->Q_prev_abs; + s->I_prev_abs = I_abs; + s->Q_prev_abs = Q_abs; + + if (I_prev_abs < 0.f) { + /* This is the first iteration, just update the prev state. */ + } else { + float P_tot; /* Total power */ + float P_n; /* Noise power */ + float P_s; /* Signal power */ + float nsr; /* Noise to signal ratio */ + float nsr_db; /* Noise to signal ratio in dB*/ + + P_s = 0.5f * (I_abs + I_prev_abs); + P_s *= P_s; + P_tot = 0.5f * (Q_prev_abs * Q_prev_abs + I_prev_abs * I_prev_abs + + Q_abs * Q_abs + I_abs * I_abs); + P_n = P_tot - P_s; + + /* Ensure the NSR is within the limit */ + if (P_s < P_n * CN0_SNV_NSR_MIN_MULTIPLIER) + nsr = CN0_SNV_NSR_MIN; + else + nsr = P_tot / P_s; + + nsr_db = 10.f * log10f(nsr); + /* Compute and store updated CN0 */ + s->cn0 = s->log_bw - nsr_db; + } + + return s->cn0; +} + +/** \} */ diff --git a/src/filter_bw2.c b/src/filter_bw2.c new file mode 100644 index 00000000..0e916835 --- /dev/null +++ b/src/filter_bw2.c @@ -0,0 +1,95 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Valeri Atamaniouk + * + * 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 + +#include +#include + +/** \defgroup track Tracking + * Functions used in tracking. + * \{ */ + +/** + * Initializes second order Butterworh lowpass IIR filter. + * + * Initializes low-pass IIR filter with transfer function: + * + * \f[ + * F(s) = \frac{\omega_0^2}{s^2 + s\sqrt{2}\omega_0 + \omega_0^2} + * \f] + * The bilinear transform is applied to obtain a digital equivalent + * \f[ + * F(z) = \frac{b + 2bz^{-1} + bz^{-2}}{1 + a_2z^{-1} + a_3z^{-2}} + * \f] + * where \f$ \omega_c = \frac{2}{T}\tan (2\pi f_{cut}\frac{T}{2}) \f$ + * and \f$ b = \frac{\omega_c^2}{1 + \sqrt{2}\omega_c + \omega_c^2} \f$ + * and \f$ a_2 = \frac{-2 + 2\omega_c^2}{1 + \sqrt{2}\omega_c + \omega_c^2} \f$ + * and \f$ a_3 = \frac{1 - \sqrt{2}\omega_c + \omega_c^2}{1 + \sqrt{2}\omega_c + \omega_c^2} \f$. + * + * \param f Filter object + * \param initial Initial value for \f$x_{n}, x_{n-1}, y_{n} and y_{n-1} \f$ + * \param cutoff_freq Filter cut-off frequency in Hz + * \param loop_freq Loop frequency in Hz + * + * \return None + */ +void bw2_filter_init(bw2_filter_t *f, + float initial, + float cutoff_freq, + float loop_freq) +{ + memset(f, 0, sizeof(*f)); + + float Tw0 = tanf(M_PI * cutoff_freq / loop_freq); + float tmp = 1.0f + sqrtf(2) * Tw0 + Tw0 * Tw0; + + f->b = Tw0 * Tw0 / tmp; + f->a2 = (-2.0f + 2.0f * Tw0 * Tw0)/tmp; + f->a3 = (1.0f - sqrtf(2) * Tw0 + Tw0 * Tw0)/tmp; + f->xn = f->xn_prev = f->yn = f->yn_prev = initial; +} + +/** + * Feeds new value into filter. + * + * Computes a value according to the transfer function: + * + * \f[ + * F(s) = \frac{\omega_0^2}{s^2 + s\sqrt{2}\omega_0 + \omega_0^2} + * \f] + * + * The bilinear transform is applied to obtain a digital equivalent: + * + * \f[ + * F(z) = \frac{b + 2bz^{-1} + bz^{-2}}{1 + a_2z^{-1} + a_3z^{-2}} + * \f] + * + * \param f Filter object + * \param value Value to filter (\f$ x_{n} \f$) + * + * \return Filtered value (\f y_{n} \f$) + */ +float bw2_filter_update(bw2_filter_t *f, float value) +{ + float yn_prev = f->yn; + f->yn = f->b * (value + f->xn * 2 + f->xn_prev) + - f->a2 * f->yn - f->a3 * f->yn_prev; + + /* Initial state is not too stable, but should be close to that */ + f->yn_prev = yn_prev; + f->xn_prev = f->xn; + f->xn = value; + return f->yn; +} + +/** \} */ diff --git a/src/filter_lp1.c b/src/filter_lp1.c new file mode 100644 index 00000000..f5b74a9e --- /dev/null +++ b/src/filter_lp1.c @@ -0,0 +1,93 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Valeri Atamaniouk + * + * 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 + +#include +#include + +/** \defgroup track Tracking + * Functions used in tracking. + * \{ */ + +/** + * Initializes second order Butterworh lowpass IIR filter. + * + * Initializes low-pass IIR filter with transfer function: + * + * \f[ + * F(s) = \frac{\omega_0}{s + \omega_0} + * \f] + * + * The bilinear transform is applied to obtain a digital equivalent + * + * \f[ + * F(z) = \frac{b + bz^{-1}}{1 + az^{-1}} + * \f] + * + * where \f$ \omega_c = \frac{2}{T}\tan (2\pi f_{cut}\frac{T}{2}) \f$ + * where \f$ b = \frac{T\omega_0}{T\omega_0 + 2} \f$ + * and \f$ a = \frac{T\omega_0 - 2}{T\omega_0 + 2} \f$. + * + * \param f Filter object + * \param initial Initial value for \f$x_{n} and y_{n} \f$ + * \param cutoff_freq Filter cut-off frequency in Hz + * \param loop_freq Loop frequency in Hz + * + * \return None + */ +void lp1_filter_init(lp1_filter_t *f, + float initial, + float cutoff_freq, + float loop_freq) +{ + memset(f, 0, sizeof(*f)); + + float Tw0 = (2 * M_PI * cutoff_freq) / loop_freq; + f->b = Tw0 / (Tw0 + 2.f); + f->a = (Tw0 - 2.f) / (Tw0 + 2.f); + + /* Set up initial filter state so that stable input produces stable output */ + f->yn = initial; + f->xn = initial * f->b; +} + +/** + * Feeds new value into filter. + * + * Computes a value according to the transfer function: + * + * \f[ + * F(s) = \frac{\omega_0}{s + \omega_0} + * \f] + * + * The bilinear transform is applied to obtain a digital equivalent + * + * \f[ + * F(z) = \frac{b + bz^{-1}}{1 + az^{-1}} + * \f] + * + * \param f Filter object + * \param value Value to filter (\f$ x_{n} \f$) + * + * \return Filtered value (\f y_{n} \f$) + */ +float lp1_filter_update(lp1_filter_t *f, float value) +{ + float tmp = f->b * value; + f->yn = tmp + f->xn - f->a * f->yn; + f->xn = tmp; + return f->yn; +} + +/** \} */ diff --git a/src/track.c b/src/track.c index 9a0f32b8..be2fb601 100644 --- a/src/track.c +++ b/src/track.c @@ -660,126 +660,6 @@ void lock_detect_update(lock_detect_t *l, float I, float Q, float DT) /** \} */ -/** 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, float bw, float cn0_0, - float cutoff_freq, float loop_freq) -{ - assert(loop_freq != 0.0f); - float Tw0 = tanf((M_PI*cutoff_freq) / loop_freq); - float tmp = 1.0+sqrtf(2)*Tw0+Tw0*Tw0; - assert(tmp != 0.0f); - - s->b = Tw0*Tw0/tmp; - s->a2 = (-2.0+2.0*Tw0*Tw0)/tmp; - s->a3 = (1.0-sqrtf(2)*Tw0+Tw0*Tw0)/tmp; - - s->log_bw = 10.f*log10f(bw); - s->I_prev_abs = -1.f; - s->Q_prev_abs = -1.f; - s->nsr = powf(10.f, 0.1f*(s->log_bw - cn0_0)); - s->nsr_prev = s->nsr; - s->xn = s->nsr; - s->xn_prev = s->nsr; -} - -/** 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 power 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 second - * order low-pass IIR filter with transfer function: - * - * \f[ - * F(s) = \frac{\omega_0^2}{s^2 + s\sqrt{2}\omega_0 + \omega_0^2} - * \f] - * The bilinear transform is applied to obtain a digital equivalent - * \f[ - * F(z) = \frac{b + 2bz^{-1} + bz^{-2}}{1 + a_2z^{-1} + a_3z^{-2}} - * \f] - * where \f$ \omega_c = \frac{2}{T}\tan (2\pi f_{cut}\frac{T}{2}) \f$ - * and \f$ b = \frac{\omega_c^2}{1 + \sqrt{2}\omega_c + \omega_c^2} \f$ - * and \f$ a_2 = \frac{-2 + 2\omega_c^2}{1 + \sqrt{2}\omega_c + \omega_c^2} \f$ - * and \f$ a_3 = \frac{1 - \sqrt{2}\omega_c + \omega_c^2}{1 + \sqrt{2}\omega_c + \omega_c^2} \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 correlator. - * \param Q The prompt quadrature correlation from the tracking correlator. - * \return The Carrier-to-Noise Density, \f$ C / N_0 \f$, in dBHz. - */ -float cn0_est(cn0_est_state_t *s, float I, float Q) -{ - 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 = fabsf(I); - s->Q_prev_abs = fabsf(Q); - } else { - P_n = fabsf(Q) - s->Q_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->Q_prev_abs = fabsf(Q); - - /* This is to avoid division by zero. */ - if (P_s == 0.0) { - return s->log_bw - 10.f*log10f(s->nsr); - } - - float tmp = P_n / P_s; - float tmp2 = s->nsr; - s->nsr = tmp * s->b + s->xn * 2*s->b + s->xn_prev * s->b \ - - s->a2 * s->nsr - s->a3 * s->nsr_prev; - s->nsr_prev = tmp2; - s->xn_prev = s->xn; - s->xn = tmp; - } - - return s->log_bw - 10.f*log10f(s->nsr); -} - void calc_navigation_measurement(u8 n_channels, const channel_measurement_t *meas[], navigation_measurement_t *nav_meas[], double nav_time, const ephemeris_t* e[]) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index f4a3abd9..cb2e4fda 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -50,6 +50,7 @@ else (CMAKE_CROSSCOMPILING) check_cnav.c check_counter_checker.c check_cn0_estimator.c + check_cn0_filters.c ) target_link_libraries(test_libswiftnav ${TEST_LIBS}) diff --git a/tests/check_cn0_estimator.c b/tests/check_cn0_estimator.c index 9e159181..c5e68261 100644 --- a/tests/check_cn0_estimator.c +++ b/tests/check_cn0_estimator.c @@ -43,7 +43,62 @@ static s8* generate_Q_input(u32 length) return input; } -START_TEST(test_cn0) +START_TEST(test_cn0_bl_init) +{ + cn0_est_state_t cn0; + cn0_est_bl_init(&cn0, 1e6f, 40.f, 1, 1); + fail_unless(cn0.cn0 == 40.f); + fail_unless(cn0.log_bw == 60.f); + fail_unless(cn0.I_prev_abs == -1.f); + fail_unless(cn0.Q_prev_abs == -1.f); + cn0_est_bl_update(&cn0, -1.f, 0.f); // I=-1; Q=0; RES=CN0_0(40) + fail_unless(cn0.I_prev_abs == 1.f); + fail_unless(cn0.Q_prev_abs == 0.f); +} +END_TEST + +START_TEST(test_cn0_bl) +{ + cn0_est_state_t s; + s8* signal_I; + s8* signal_Q; + u32 ii = 0; + u32 test_length = 1000; + float cn0 = 0.0; + + signal_I = generate_I_input(test_length); + fail_if(NULL == signal_I, "Could not allocate I data"); + signal_Q = generate_Q_input(test_length); + fail_if(NULL == signal_Q, "Could not allocate Q data"); + + cn0_est_bl_init(&s, BW, CN0_0, CUTOFF_FREQ, LOOP_FREQ); + + for(ii = 0; ii < test_length; ii++) { + cn0 = cn0_est_bl_update(&s, signal_I[ii], signal_Q[ii]); + } + + fail_if(cn0 < 30.0); + + free(signal_I); + free(signal_Q); +} +END_TEST + +START_TEST(test_cn0_snv_init) +{ + cn0_est_state_t cn0; + cn0_est_snv_init(&cn0, 1e6f, 40.f, 1, 1); + fail_unless(cn0.cn0 == 40.f); + fail_unless(cn0.log_bw == 60.f); + fail_unless(cn0.I_prev_abs == -1.f); + fail_unless(cn0.Q_prev_abs == -1.f); + cn0_est_snv_update(&cn0, -1.f, 0.f); // I=-1; Q=0; RES=CN0_0(40) + fail_unless(cn0.I_prev_abs == 1.f); + fail_unless(cn0.Q_prev_abs == 0.f); +} +END_TEST + +START_TEST(test_cn0_snv) { cn0_est_state_t s; s8* signal_I; @@ -57,10 +112,10 @@ START_TEST(test_cn0) signal_Q = generate_Q_input(test_length); fail_if(NULL == signal_Q, "Could not allocate Q data"); - cn0_est_init(&s, BW, CN0_0, CUTOFF_FREQ, LOOP_FREQ); + cn0_est_snv_init(&s, BW, CN0_0, CUTOFF_FREQ, LOOP_FREQ); for(ii = 0; ii < test_length; ii++) { - cn0 = cn0_est(&s, signal_I[ii], signal_Q[ii]); + cn0 = cn0_est_snv_update(&s, signal_I[ii], signal_Q[ii]); } fail_if(cn0 < 30.0); @@ -75,7 +130,10 @@ Suite* cn0_suite(void) Suite *s = suite_create("CN0"); TCase *tc_core = tcase_create("Core"); - tcase_add_test(tc_core, test_cn0); + tcase_add_test(tc_core, test_cn0_bl_init); + tcase_add_test(tc_core, test_cn0_bl); + tcase_add_test(tc_core, test_cn0_snv_init); + tcase_add_test(tc_core, test_cn0_snv); suite_add_tcase(s, tc_core); return s; diff --git a/tests/check_cn0_filters.c b/tests/check_cn0_filters.c new file mode 100644 index 00000000..d867a9c3 --- /dev/null +++ b/tests/check_cn0_filters.c @@ -0,0 +1,155 @@ +#include +#include +#include +#include +#include + +#define BW 1000 +#define CN0_0 40 +#define CUTOFF_FREQ 0.1 +#define LOOP_FREQ 1000 + + +START_TEST(test_lp1_init1) +{ + lp1_filter_t filter; + lp1_filter_init(&filter, 0.f, 0.5f, 1e3f); + + fail_unless(filter.xn == 0.f); + fail_unless(filter.yn == 0.f); + fail_unless(filter.a != 0.f); + fail_unless(filter.b != 0.f); +} +END_TEST + +START_TEST(test_lp1_init2) +{ + lp1_filter_t filter; + lp1_filter_init(&filter, 1.f, 0.5f, 1e3f); + + fail_unless(filter.xn == 1.f * filter.b); + fail_unless(filter.yn == 1.f); + fail_unless(filter.a != 0.f); + fail_unless(filter.b != 0.f); +} +END_TEST + +START_TEST(test_lp1_update1) +{ + lp1_filter_t filter; + lp1_filter_init(&filter, 1.f, 0.5f, 1e3f); + float r; + + r = lp1_filter_update(&filter, 1.); + fail_unless(r == 1.f); +} +END_TEST + +START_TEST(test_lp1_update2) +{ + lp1_filter_t filter; + lp1_filter_init(&filter, 1.f, 0.5f, 1e3f); + float r; + + r = lp1_filter_update(&filter, 0.f); + fail_unless(r < 1.f); +} +END_TEST + +START_TEST(test_lp1_update3) +{ + lp1_filter_t filter; + lp1_filter_init(&filter, 1.f, 0.5f, 1e3f); + float r; + + r = lp1_filter_update(&filter, 1.1f); + fail_unless(r > 1.f); +} +END_TEST + +START_TEST(test_bw2_init1) +{ + bw2_filter_t filter; + bw2_filter_init(&filter, 0.f, 0.5f, 1e3f); + + fail_unless(filter.xn == 0.f); + fail_unless(filter.xn_prev == 0.f); + fail_unless(filter.yn == 0.f); + fail_unless(filter.yn_prev == 0.f); + fail_unless(filter.a2 != 0.f); + fail_unless(filter.a3 != 0.f); + fail_unless(filter.b != 0.f); +} +END_TEST + +START_TEST(test_bw2_init2) +{ + bw2_filter_t filter; + bw2_filter_init(&filter, 1.f, 0.5f, 1e3f); + + fail_unless(filter.xn == 1.f); + fail_unless(filter.xn_prev == 1.f); + fail_unless(filter.yn == 1.f); + fail_unless(filter.yn_prev == 1.f); + fail_unless(filter.a2 != 0.f); + fail_unless(filter.a3 != 0.f); + fail_unless(filter.b != 0.f); +} +END_TEST + +START_TEST(test_bw2_update1) +{ + bw2_filter_t filter; + float r; + + bw2_filter_init(&filter, 1.f, 0.5f, 1e3f); + r = bw2_filter_update(&filter, 1.f); + + fail_unless(fabsf(r - 1.f) < 1e-6); +} +END_TEST + +START_TEST(test_bw2_update2) +{ + bw2_filter_t filter; + float r; + + bw2_filter_init(&filter, 1.f, 0.5f, 1e3f); + r = bw2_filter_update(&filter, .9); + + fail_unless(r < 1.f); +} +END_TEST + +START_TEST(test_bw2_update3) +{ + bw2_filter_t filter; + float r; + + bw2_filter_init(&filter, 1.f, 0.5f, 1e3f); + r = bw2_filter_update(&filter, 1.1f); + + fail_unless(r > 1.f); +} +END_TEST + + +Suite* cn0_filter_suite(void) +{ + Suite *s = suite_create("CN0 Filters"); + TCase *tc_core = tcase_create("Core"); + + tcase_add_test(tc_core, test_lp1_init1); + tcase_add_test(tc_core, test_lp1_init2); + tcase_add_test(tc_core, test_lp1_update1); + tcase_add_test(tc_core, test_lp1_update2); + tcase_add_test(tc_core, test_lp1_update3); + tcase_add_test(tc_core, test_bw2_init1); + tcase_add_test(tc_core, test_bw2_init2); + tcase_add_test(tc_core, test_bw2_update1); + tcase_add_test(tc_core, test_bw2_update2); + tcase_add_test(tc_core, test_bw2_update3); + suite_add_tcase(s, tc_core); + + return s; +} diff --git a/tests/check_main.c b/tests/check_main.c index 128de18b..a9c9337a 100644 --- a/tests/check_main.c +++ b/tests/check_main.c @@ -36,6 +36,7 @@ int main(void) srunner_add_suite(sr, correlator_suite()); srunner_add_suite(sr, counter_checker_suite()); srunner_add_suite(sr, cn0_suite()); + srunner_add_suite(sr, cn0_filter_suite()); srunner_set_fork_status(sr, CK_NOFORK); srunner_run_all(sr, CK_NORMAL); diff --git a/tests/check_suites.h b/tests/check_suites.h index 1ee9cfd3..e8b52ef2 100644 --- a/tests/check_suites.h +++ b/tests/check_suites.h @@ -26,5 +26,6 @@ Suite* cnav_test_suite(void); Suite* correlator_suite(void); Suite* counter_checker_suite(void); Suite* cn0_suite(void); +Suite* cn0_filter_suite(void); #endif /* CHECK_SUITES_H */