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,