From a18c3467a18661f9f1b17176f73a7ea75e39a5cc Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 22 Aug 2017 13:21:28 +0200 Subject: [PATCH 1/2] Improved Notch Filter Lite Updated version of the filter --- .../input_filter/adapters/notch_filter.cc | 8 +- .../adapters/notch_filter_lite.cc | 23 +++- .../input_filter/gnuradio_blocks/notch_cc.cc | 10 +- .../input_filter/gnuradio_blocks/notch_cc.h | 4 +- .../gnuradio_blocks/notch_lite_cc.cc | 105 +++++++++++++++--- .../gnuradio_blocks/notch_lite_cc.h | 24 +++- 6 files changed, 142 insertions(+), 32 deletions(-) diff --git a/src/algorithms/input_filter/adapters/notch_filter.cc b/src/algorithms/input_filter/adapters/notch_filter.cc index a69c0c9c0..405f1f430 100644 --- a/src/algorithms/input_filter/adapters/notch_filter.cc +++ b/src/algorithms/input_filter/adapters/notch_filter.cc @@ -53,6 +53,10 @@ NotchFilter::NotchFilter(ConfigurationInterface* configuration, std::string role float default_p_c_factor = 0.9; int length_; int default_length_ = 32; + int n_segments_est; + int default_n_segments_est = 12500; + int n_segments_reset; + int default_n_segments_reset = 5000000; std::string default_item_type = "gr_complex"; std::string default_dump_file = "./data/input_filter.dat"; item_type_ = configuration->property(role + ".item_type", default_item_type); @@ -62,10 +66,12 @@ NotchFilter::NotchFilter(ConfigurationInterface* configuration, std::string role pfa = configuration->property(role + ".pfa", default_pfa); p_c_factor = configuration->property(role + ".p_c_factor", default_p_c_factor); length_ = configuration->property(role + ".length", default_length_); + n_segments_est = configuration->property(role + ".segments_est", default_n_segments_est); + n_segments_reset = configuration->property(role + ".segments_reset", default_n_segments_reset); if (item_type_.compare("gr_complex") == 0) { item_size_ = sizeof(gr_complex); - notch_filter_ = make_notch_filter(pfa, p_c_factor, length_); + notch_filter_ = make_notch_filter(pfa, p_c_factor, length_, n_segments_est, n_segments_reset); DLOG(INFO) << "Item size " << item_size_; DLOG(INFO) << "input filter(" << notch_filter_->unique_id() << ")"; diff --git a/src/algorithms/input_filter/adapters/notch_filter_lite.cc b/src/algorithms/input_filter/adapters/notch_filter_lite.cc index ec2ac636e..a91aa39e6 100644 --- a/src/algorithms/input_filter/adapters/notch_filter_lite.cc +++ b/src/algorithms/input_filter/adapters/notch_filter_lite.cc @@ -31,6 +31,7 @@ #include "notch_filter_lite.h" #include +#include #include #include #include @@ -49,6 +50,18 @@ NotchFilterLite::NotchFilterLite(ConfigurationInterface* configuration, std::str size_t item_size_; float p_c_factor; float default_p_c_factor = 0.9; + float pfa; + float default_pfa = 0.001; + int length_; + int default_length_ = 32; + int n_segments_est; + int default_n_segments_est = 12500; + int n_segments_reset; + int default_n_segments_reset = 5000000; + float default_samp_freq = 4000000; + float samp_freq = configuration->property("SignalSource.sampling_frequency", default_samp_freq); + float default_coeff_rate = samp_freq * 0.1; + float coeff_rate; std::string default_item_type = "gr_complex"; std::string default_dump_file = "./data/input_filter.dat"; item_type_ = configuration->property(role + ".item_type", default_item_type); @@ -56,13 +69,19 @@ NotchFilterLite::NotchFilterLite(ConfigurationInterface* configuration, std::str DLOG(INFO) << "dump_ is " << dump_; dump_filename_ = configuration->property(role + ".dump_filename", default_dump_file); p_c_factor = configuration->property(role + ".p_c_factor", default_p_c_factor); + pfa = configuration->property(role + ".pfa", default_pfa); + coeff_rate = configuration->property(role + ".coeff_rate", default_coeff_rate); + length_ = configuration->property(role + ".length", default_length_); + n_segments_est = configuration->property(role + ".segments_est", default_n_segments_est); + n_segments_reset = configuration->property(role + ".segments_reset", default_n_segments_reset); + int n_segments_coeff = (int) ((samp_freq / coeff_rate) / ((float) length_)); + n_segments_coeff = std::max(1, n_segments_coeff); if (item_type_.compare("gr_complex") == 0) { item_size_ = sizeof(gr_complex); - notch_filter_lite_ = make_notch_filter_lite(p_c_factor); + notch_filter_lite_ = make_notch_filter_lite(p_c_factor, pfa, length_, n_segments_est, n_segments_reset, n_segments_coeff); DLOG(INFO) << "Item size " << item_size_; DLOG(INFO) << "input filter(" << notch_filter_lite_->unique_id() << ")"; - } else { diff --git a/src/algorithms/input_filter/gnuradio_blocks/notch_cc.cc b/src/algorithms/input_filter/gnuradio_blocks/notch_cc.cc index 007b3d9b3..700ccfea8 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/notch_cc.cc +++ b/src/algorithms/input_filter/gnuradio_blocks/notch_cc.cc @@ -43,12 +43,12 @@ using google::LogMessage; notch_sptr make_notch_filter(float pfa, float p_c_factor, - int length_) + int length_, int n_segments_est, int n_segments_reset) { - return notch_sptr(new Notch(pfa, p_c_factor, length_)); + return notch_sptr(new Notch(pfa, p_c_factor, length_, n_segments_est, n_segments_reset)); } -Notch::Notch(float pfa, float p_c_factor, int length_) : gr::block("Notch", +Notch::Notch(float pfa, float p_c_factor, int length_, int n_segments_est, int n_segments_reset) : gr::block("Notch", gr::io_signature::make (1, 1, sizeof(gr_complex)), gr::io_signature::make (1, 1, sizeof(gr_complex))) { @@ -63,8 +63,8 @@ Notch::Notch(float pfa, float p_c_factor, int length_) : gr::block("Notch", filter_state_ = false; //Initial state of the filter n_deg_fred = 2 * length_; //Number of dregrees of freedom n_segments = 0; - n_segments_est = 8; // Set the number of segments for noise power estimation - n_segments_reset = 10000; // Set the period (in segments) when the noise power is estimated + this->n_segments_est = n_segments_est; // Set the number of segments for noise power estimation + this->n_segments_reset = n_segments_reset; // Set the period (in segments) when the noise power is estimated z_0 = gr_complex(0 , 0); boost::math::chi_squared_distribution my_dist_(n_deg_fred); thres_ = boost::math::quantile(boost::math::complement(my_dist_, pfa)); diff --git a/src/algorithms/input_filter/gnuradio_blocks/notch_cc.h b/src/algorithms/input_filter/gnuradio_blocks/notch_cc.h index faf7c5d0c..5be7dfda6 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/notch_cc.h +++ b/src/algorithms/input_filter/gnuradio_blocks/notch_cc.h @@ -39,7 +39,7 @@ class Notch; typedef boost::shared_ptr notch_sptr; notch_sptr make_notch_filter(float pfa, float p_c_factor, - int length_); + int length_, int n_segments_est, int n_segments_reset); /*! * \brief This class implements a real-time software-defined multi state notch filter @@ -67,7 +67,7 @@ private: public: - Notch(float pfa, float p_c_factor, int length_); + Notch(float pfa, float p_c_factor, int length_, int n_segments_est, int n_segments_reset); ~Notch(); diff --git a/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.cc b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.cc index 4b21bf33e..3ce19f656 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.cc +++ b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.cc @@ -37,15 +37,17 @@ #include #include #include +#include +#include using google::LogMessage; -notch_lite_sptr make_notch_filter_lite(float p_c_factor) +notch_lite_sptr make_notch_filter_lite(float p_c_factor, float pfa, int length_, int n_segments_est, int n_segments_reset, int n_segments_coeff) { - return notch_lite_sptr(new NotchLite(p_c_factor)); + return notch_lite_sptr(new NotchLite(p_c_factor, pfa, length_, n_segments_est, n_segments_reset, n_segments_coeff)); } -NotchLite::NotchLite(float p_c_factor) : gr::block("NotchLite", +NotchLite::NotchLite(float p_c_factor, float pfa, int length_, int n_segments_est, int n_segments_reset, int n_segments_coeff) : gr::block("NotchLite", gr::io_signature::make (1, 1, sizeof(gr_complex)), gr::io_signature::make (1, 1, sizeof(gr_complex))) { @@ -53,28 +55,97 @@ NotchLite::NotchLite(float p_c_factor) : gr::block("NotchLite", set_alignment(std::max(1, alignment_multiple)); set_history(2); this->p_c_factor = gr_complex(p_c_factor , 0); + this->n_segments_est = n_segments_est; + this->n_segments_reset = n_segments_reset; + this->n_segments_coeff_reset = n_segments_coeff; + this->n_segments_coeff = 0; + this->length_ = length_; + set_output_multiple(length_); + this->pfa = pfa; + n_segments = 0; + n_deg_fred = 2 * length_; + noise_pow_est = 0.0; + filter_state_ = false; z_0 = gr_complex(0 , 0); - last_out = gr_complex(0,0); + last_out = gr_complex(0, 0); + boost::math::chi_squared_distribution my_dist_(n_deg_fred); + thres_ = boost::math::quantile(boost::math::complement(my_dist_, pfa)); + c_samples = gr_complex(0, 0); + angle_ = 0.0; + power_spect = static_cast(volk_malloc(length_ * sizeof(float), volk_get_alignment())); + } +NotchLite::~NotchLite() +{ + volk_free(power_spect); +} + + int NotchLite::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { + int index_out = 0; + float sig2dB = 0.0; + float sig2lin = 0.0; + lv_32fc_t dot_prod_; gr_complex* in = (gr_complex *) input_items[0]; gr_complex* out = (gr_complex *) output_items[0]; - c_samples = static_cast(volk_malloc(noutput_items * sizeof(gr_complex), volk_get_alignment())); - angle_ = static_cast(volk_malloc(noutput_items * sizeof(float), volk_get_alignment())); - - volk_32fc_x2_multiply_conjugate_32fc(c_samples, (in + 1), in, noutput_items); - volk_32fc_s32f_atan2_32f(angle_, c_samples, ((float)1.0), noutput_items); - for (int aux = 0; aux < noutput_items; aux++) + in++; + arma::cx_fvec signal_segment; + arma::cx_fvec signal_segment_fft; + while((index_out + length_) < noutput_items) { - z_0 = std::exp(gr_complex(0,1) * (*(angle_ + aux))); - *(out + aux) = *(in + aux + 1) - z_0 * (*(in + aux)) + p_c_factor * z_0 * last_out; - last_out = *(out + aux); + if((n_segments < n_segments_est) && (filter_state_ == false)) + { + signal_segment = arma::cx_fvec(in, length_, false, false); + signal_segment_fft = arma::fft(signal_segment); + volk_32fc_s32f_power_spectrum_32f(power_spect, signal_segment_fft.memptr(), 1.0, length_); + volk_32f_s32f_calc_spectral_noise_floor_32f(&sig2dB, power_spect, 15.0, length_); + sig2lin = std::pow(10.0, (sig2dB / 10.0)) / ((float) n_deg_fred); + noise_pow_est = (((float) n_segments) * noise_pow_est + sig2lin) / ((float)(n_segments + 1)); + memcpy(out, in, sizeof(gr_complex) * length_); + } + else + { + volk_32fc_x2_conjugate_dot_prod_32fc(&dot_prod_, in, in, length_); + if( (lv_creal(dot_prod_) / noise_pow_est) > thres_) + { + if(filter_state_ == false) + { + filter_state_ = true; + last_out = gr_complex(0,0); + n_segments_coeff = 0; + } + if(n_segments_coeff == 0) + { + volk_32fc_x2_multiply_conjugate_32fc(&c_samples, in, (in - 1), 1); + volk_32fc_s32f_atan2_32f(&angle_, &c_samples, ((float)1.0), 1); + z_0 = std::exp(gr_complex(0,1) * angle_); + } + for(int aux = 0; aux < length_; aux++) + { + *(out + aux) = *(in + aux) - z_0 * (*(in + aux - 1)) + p_c_factor * z_0 * last_out; + last_out = *(out + aux); + } + n_segments_coeff++; + n_segments_coeff = n_segments_coeff % n_segments_coeff_reset; + } + else + { + if (n_segments > n_segments_reset) + { + n_segments = 0; + } + filter_state_ = false; + memcpy(out, in, sizeof(gr_complex) * length_); + } + } + index_out += length_; + n_segments++; + in += length_; + out += length_; } - volk_free(c_samples); - volk_free(angle_); - consume_each(noutput_items); - return noutput_items; + consume_each(index_out); + return index_out; } diff --git a/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h index 16f56286d..64dcb8d53 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h +++ b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h @@ -38,7 +38,7 @@ class NotchLite; typedef boost::shared_ptr notch_lite_sptr; -notch_lite_sptr make_notch_filter_lite(float p_c_factor); +notch_lite_sptr make_notch_filter_lite(float p_c_factor, float pfa, int length_, int n_segments_est, int n_segments_reset, int n_segments_coeff); /*! * \brief This class implements a real-time software-defined single state notch filter @@ -48,15 +48,29 @@ class NotchLite : public gr::block { private: + int length_; + int n_segments; + int n_segments_est; + int n_segments_reset; + int n_segments_coeff_reset; + int n_segments_coeff; + int n_deg_fred; + float pfa; + float thres_; + float noise_pow_est; + bool filter_state_; gr_complex last_out; gr_complex z_0; gr_complex p_c_factor; - gr_complex* c_samples; - float* angle_; - + gr_complex c_samples; + float angle_; + float* power_spect; + public: - NotchLite(float p_c_factor); + NotchLite(float p_c_factor, float pfa, int length_, int n_segments_est, int n_segments_reset, int n_segments_coeff); + + ~NotchLite(); int general_work (int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, From 0441f3c24c57601ebb21a09a7513e7aac4165d16 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 24 Aug 2017 19:21:24 +0200 Subject: [PATCH 2/2] Minor changes Changing some variable names --- src/algorithms/input_filter/adapters/notch_filter.h | 2 +- .../input_filter/adapters/notch_filter_lite.h | 2 +- .../input_filter/adapters/pulse_blanking_filter.cc | 4 ++-- .../input_filter/adapters/pulse_blanking_filter.h | 1 + .../input_filter/gnuradio_blocks/notch_lite_cc.cc | 13 +++++++++---- .../input_filter/gnuradio_blocks/notch_lite_cc.h | 10 ++++++---- .../gnuradio_blocks/pulse_blanking_cc.cc | 4 ++-- .../gnuradio_blocks/pulse_blanking_cc.h | 4 ++-- 8 files changed, 24 insertions(+), 16 deletions(-) diff --git a/src/algorithms/input_filter/adapters/notch_filter.h b/src/algorithms/input_filter/adapters/notch_filter.h index 09ac10e99..a8ab2136b 100644 --- a/src/algorithms/input_filter/adapters/notch_filter.h +++ b/src/algorithms/input_filter/adapters/notch_filter.h @@ -1,6 +1,6 @@ /*! * \file notch_filter.h - * \brief + * \brief Adapter of a multistate Notch filter * \author Antonio Ramos, 2017. antonio.ramosdet(at)gmail.com * * Detailed description of the file here if needed. diff --git a/src/algorithms/input_filter/adapters/notch_filter_lite.h b/src/algorithms/input_filter/adapters/notch_filter_lite.h index 03796f319..f9088db52 100644 --- a/src/algorithms/input_filter/adapters/notch_filter_lite.h +++ b/src/algorithms/input_filter/adapters/notch_filter_lite.h @@ -1,6 +1,6 @@ /*! * \file notch_filter_lite.h - * \brief + * \brief Adapts a ligth version of a multistate notch filter * \author Antonio Ramos, 2017. antonio.ramosdet(at)gmail.com * * Detailed description of the file here if needed. diff --git a/src/algorithms/input_filter/adapters/pulse_blanking_filter.cc b/src/algorithms/input_filter/adapters/pulse_blanking_filter.cc index fed7205cc..0786f3f3e 100644 --- a/src/algorithms/input_filter/adapters/pulse_blanking_filter.cc +++ b/src/algorithms/input_filter/adapters/pulse_blanking_filter.cc @@ -2,7 +2,7 @@ * \file pulse_blanking_filter.cc * \brief Instantiates the GNSS-SDR pulse blanking filter * \author Javier Arribas 2017 - * + * Antonio Ramos 2017 * ------------------------------------------------------------------------- * * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) @@ -57,7 +57,7 @@ PulseBlankingFilter::PulseBlankingFilter(ConfigurationInterface* configuration, int default_length_ = 32; int length_ = config_->property(role_ + ".length", default_length_); int default_n_segments_est = 12500; - int n_segments_est = config_->property(role_ + ".segments_estimation", default_n_segments_est); + int n_segments_est = config_->property(role_ + ".segments_est", default_n_segments_est); int default_n_segments_reset = 5000000; int n_segments_reset = config_->property(role_ + ".segments_reset", default_n_segments_reset); if (input_item_type_.compare("gr_complex") == 0) diff --git a/src/algorithms/input_filter/adapters/pulse_blanking_filter.h b/src/algorithms/input_filter/adapters/pulse_blanking_filter.h index 041ba2251..c57c68d10 100644 --- a/src/algorithms/input_filter/adapters/pulse_blanking_filter.h +++ b/src/algorithms/input_filter/adapters/pulse_blanking_filter.h @@ -2,6 +2,7 @@ * \file pulse_blanking_filter.h * \brief Instantiates the GNSS-SDR pulse blanking filter * \author Javier Arribas 2017 + * Antonio Ramos 2017 * * ------------------------------------------------------------------------- * diff --git a/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.cc b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.cc index 3ce19f656..a84ec6ae6 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.cc +++ b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.cc @@ -70,8 +70,10 @@ NotchLite::NotchLite(float p_c_factor, float pfa, int length_, int n_segments_es last_out = gr_complex(0, 0); boost::math::chi_squared_distribution my_dist_(n_deg_fred); thres_ = boost::math::quantile(boost::math::complement(my_dist_, pfa)); - c_samples = gr_complex(0, 0); - angle_ = 0.0; + c_samples1 = gr_complex(0, 0); + c_samples2 = gr_complex(0, 0); + angle1 = 0.0; + angle2 = 0.0; power_spect = static_cast(volk_malloc(length_ * sizeof(float), volk_get_alignment())); } @@ -119,8 +121,11 @@ int NotchLite::general_work(int noutput_items __attribute__((unused)), gr_vector } if(n_segments_coeff == 0) { - volk_32fc_x2_multiply_conjugate_32fc(&c_samples, in, (in - 1), 1); - volk_32fc_s32f_atan2_32f(&angle_, &c_samples, ((float)1.0), 1); + volk_32fc_x2_multiply_conjugate_32fc(&c_samples1, (in + 1), in, 1); + volk_32fc_s32f_atan2_32f(&angle1, &c_samples1, ((float)1.0), 1); + volk_32fc_x2_multiply_conjugate_32fc(&c_samples2, (in + length_ - 1), (in + length_ - 2), 1); + volk_32fc_s32f_atan2_32f(&angle2, &c_samples2, ((float)1.0), 1); + float angle_ = (angle1 + angle2) / 2.0; z_0 = std::exp(gr_complex(0,1) * angle_); } for(int aux = 0; aux < length_; aux++) diff --git a/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h index 64dcb8d53..2151a459f 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h +++ b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h @@ -1,6 +1,6 @@ /*! * \file notch_lite_cc.h - * \brief Implements a notch filter algorithm + * \brief Implements a notch filter ligth algorithm * \author Antonio Ramos (antonio.ramosdet(at)gmail.com) * * ------------------------------------------------------------------------- @@ -41,7 +41,7 @@ typedef boost::shared_ptr notch_lite_sptr; notch_lite_sptr make_notch_filter_lite(float p_c_factor, float pfa, int length_, int n_segments_est, int n_segments_reset, int n_segments_coeff); /*! - * \brief This class implements a real-time software-defined single state notch filter + * \brief This class implements a real-time software-defined multi state notch filter ligth version */ class NotchLite : public gr::block @@ -62,8 +62,10 @@ private: gr_complex last_out; gr_complex z_0; gr_complex p_c_factor; - gr_complex c_samples; - float angle_; + gr_complex c_samples1; + gr_complex c_samples2; + float angle1; + float angle2; float* power_spect; public: diff --git a/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.cc b/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.cc index a744f8747..7924f17ba 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.cc +++ b/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.cc @@ -1,8 +1,8 @@ /*! * \file pulse_blanking_cc.cc - * \brief Implements a simple pulse blanking algorithm + * \brief Implements a pulse blanking algorithm * \author Javier Arribas (jarribas(at)cttc.es) - * + * Antonio Ramos (antonio.ramosdet(at)gmail.com) * ------------------------------------------------------------------------- * * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) diff --git a/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h b/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h index a77cf4deb..7a19bf74e 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h +++ b/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h @@ -1,8 +1,8 @@ /*! * \file pulse_blanking_cc.h - * \brief Implements a simple pulse blanking algorithm + * \brief Implements a pulse blanking algorithm * \author Javier Arribas (jarribas(at)cttc.es) - * + * Antonio Ramos (antonio.ramosdet(at)gmail.com) * ------------------------------------------------------------------------- * * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors)