diff --git a/src/algorithms/input_filter/adapters/CMakeLists.txt b/src/algorithms/input_filter/adapters/CMakeLists.txt index 2631239a7..267ad9062 100644 --- a/src/algorithms/input_filter/adapters/CMakeLists.txt +++ b/src/algorithms/input_filter/adapters/CMakeLists.txt @@ -21,6 +21,8 @@ set(INPUT_FILTER_ADAPTER_SOURCES freq_xlating_fir_filter.cc beamformer_filter.cc pulse_blanking_filter.cc + notch_filter.cc + notch_filter_lite.cc ) include_directories( diff --git a/src/algorithms/input_filter/adapters/notch_filter.cc b/src/algorithms/input_filter/adapters/notch_filter.cc new file mode 100644 index 000000000..405f1f430 --- /dev/null +++ b/src/algorithms/input_filter/adapters/notch_filter.cc @@ -0,0 +1,126 @@ +/*! + * \file notch_filter.cc + * \brief Adapts a gnuradio gr_notch_filter + * \author Antonio Ramos, 2017. antonio.ramosdet(at)gmail.com + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "notch_filter.h" +#include +#include +#include +#include +#include +#include +#include "configuration_interface.h" +#include "notch_cc.h" + +using google::LogMessage; + +NotchFilter::NotchFilter(ConfigurationInterface* configuration, std::string role, + unsigned int in_streams, unsigned int out_streams) : + role_(role), in_streams_(in_streams), + out_streams_(out_streams) +{ + size_t item_size_; + float pfa; + float default_pfa = 0.001; + float p_c_factor; + 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); + dump_ = configuration->property(role + ".dump", false); + DLOG(INFO) << "dump_ is " << dump_; + dump_filename_ = configuration->property(role + ".dump_filename", default_dump_file); + 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_, n_segments_est, n_segments_reset); + DLOG(INFO) << "Item size " << item_size_; + DLOG(INFO) << "input filter(" << notch_filter_->unique_id() << ")"; + + } + else + { + LOG(WARNING) << item_type_ + << " unrecognized item type for notch filter"; + item_size_ = sizeof(gr_complex); + } + if (dump_) + { + DLOG(INFO) << "Dumping output into file " << dump_filename_; + file_sink_ = gr::blocks::file_sink::make(item_size_, dump_filename_.c_str()); + DLOG(INFO) << "file_sink(" << file_sink_->unique_id() << ")"; + } +} + +NotchFilter::~NotchFilter() +{} + +void NotchFilter::connect(gr::top_block_sptr top_block) +{ + if (dump_) + { + top_block->connect(notch_filter_, 0, file_sink_, 0); + DLOG(INFO) << "connected notch filter output to file sink"; + } + else + { + DLOG(INFO) << "nothing to connect internally"; + } +} + +void NotchFilter::disconnect(gr::top_block_sptr top_block) +{ + if (dump_) + { + top_block->disconnect(notch_filter_, 0, file_sink_, 0); + } +} + + +gr::basic_block_sptr NotchFilter::get_left_block() +{ + return notch_filter_; +} + +gr::basic_block_sptr NotchFilter::get_right_block() +{ + return notch_filter_; +} diff --git a/src/algorithms/input_filter/adapters/notch_filter.h b/src/algorithms/input_filter/adapters/notch_filter.h new file mode 100644 index 000000000..a8ab2136b --- /dev/null +++ b/src/algorithms/input_filter/adapters/notch_filter.h @@ -0,0 +1,84 @@ +/*! + * \file notch_filter.h + * \brief Adapter of a multistate Notch filter + * \author Antonio Ramos, 2017. antonio.ramosdet(at)gmail.com + * + * Detailed description of the file here if needed. + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_NOTCH_FILTER_H_ +#define GNSS_SDR_NOTCH_FILTER_H_ + +#include +#include +#include +#include "gnss_block_interface.h" +#include "notch_cc.h" + + +class ConfigurationInterface; + +class NotchFilter: public GNSSBlockInterface +{ +public: + NotchFilter(ConfigurationInterface* configuration, + std::string role, unsigned int in_streams, + unsigned int out_streams); + + virtual ~NotchFilter(); + std::string role() + { + return role_; + } + + //! Returns "Notch_Filter" + std::string implementation() + { + return "Notch_Filter"; + } + size_t item_size() + { + return 0; + } + void connect(gr::top_block_sptr top_block); + void disconnect(gr::top_block_sptr top_block); + gr::basic_block_sptr get_left_block(); + gr::basic_block_sptr get_right_block(); + +private: + + bool dump_; + std::string dump_filename_; + std::string role_; + std::string item_type_; + unsigned int in_streams_; + unsigned int out_streams_; + gr::blocks::file_sink::sptr file_sink_; + notch_sptr notch_filter_; +}; + +#endif //GNSS_SDR_NOTCH_FILTER_H_ diff --git a/src/algorithms/input_filter/adapters/notch_filter_lite.cc b/src/algorithms/input_filter/adapters/notch_filter_lite.cc new file mode 100644 index 000000000..a91aa39e6 --- /dev/null +++ b/src/algorithms/input_filter/adapters/notch_filter_lite.cc @@ -0,0 +1,133 @@ +/*! + * \file notch_filter_lite.cc + * \brief Adapts a gnuradio gr_notch_filter_lite + * \author Antonio Ramos, 2017. antonio.ramosdet(at)gmail.com + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "notch_filter_lite.h" +#include +#include +#include +#include +#include +#include +#include +#include "configuration_interface.h" +#include "notch_lite_cc.h" + +using google::LogMessage; + +NotchFilterLite::NotchFilterLite(ConfigurationInterface* configuration, std::string role, + unsigned int in_streams, unsigned int out_streams) : + role_(role), in_streams_(in_streams), + out_streams_(out_streams) +{ + 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); + dump_ = configuration->property(role + ".dump", false); + 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, 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 + { + LOG(WARNING) << item_type_ + << " unrecognized item type for notch filter"; + item_size_ = sizeof(gr_complex); + } + if (dump_) + { + DLOG(INFO) << "Dumping output into file " << dump_filename_; + file_sink_ = gr::blocks::file_sink::make(item_size_, dump_filename_.c_str()); + DLOG(INFO) << "file_sink(" << file_sink_->unique_id() << ")"; + } +} + +NotchFilterLite::~NotchFilterLite() +{} + +void NotchFilterLite::connect(gr::top_block_sptr top_block) +{ + if (dump_) + { + top_block->connect(notch_filter_lite_, 0, file_sink_, 0); + DLOG(INFO) << "connected notch filter output to file sink"; + } + else + { + DLOG(INFO) << "nothing to connect internally"; + } +} + +void NotchFilterLite::disconnect(gr::top_block_sptr top_block) +{ + if (dump_) + { + top_block->disconnect(notch_filter_lite_, 0, file_sink_, 0); + } +} + + +gr::basic_block_sptr NotchFilterLite::get_left_block() +{ + return notch_filter_lite_; +} + +gr::basic_block_sptr NotchFilterLite::get_right_block() +{ + return notch_filter_lite_; +} diff --git a/src/algorithms/input_filter/adapters/notch_filter_lite.h b/src/algorithms/input_filter/adapters/notch_filter_lite.h new file mode 100644 index 000000000..f9088db52 --- /dev/null +++ b/src/algorithms/input_filter/adapters/notch_filter_lite.h @@ -0,0 +1,84 @@ +/*! + * \file notch_filter_lite.h + * \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. + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_NOTCH_FILTER_LITE_H_ +#define GNSS_SDR_NOTCH_FILTER_LITE_H_ + +#include +#include +#include +#include "gnss_block_interface.h" +#include "notch_lite_cc.h" + + +class ConfigurationInterface; + +class NotchFilterLite: public GNSSBlockInterface +{ +public: + NotchFilterLite(ConfigurationInterface* configuration, + std::string role, unsigned int in_streams, + unsigned int out_streams); + + virtual ~NotchFilterLite(); + std::string role() + { + return role_; + } + + //! Returns "Notch_Filter_Lite" + std::string implementation() + { + return "Notch_Filter_Lite"; + } + size_t item_size() + { + return 0; + } + void connect(gr::top_block_sptr top_block); + void disconnect(gr::top_block_sptr top_block); + gr::basic_block_sptr get_left_block(); + gr::basic_block_sptr get_right_block(); + +private: + + bool dump_; + std::string dump_filename_; + std::string role_; + std::string item_type_; + unsigned int in_streams_; + unsigned int out_streams_; + gr::blocks::file_sink::sptr file_sink_; + notch_lite_sptr notch_filter_lite_; +}; + +#endif //GNSS_SDR_NOTCH_FILTER_LITE_H_ diff --git a/src/algorithms/input_filter/adapters/pulse_blanking_filter.cc b/src/algorithms/input_filter/adapters/pulse_blanking_filter.cc index de0fc59f3..72a98d576 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) @@ -42,25 +42,29 @@ PulseBlankingFilter::PulseBlankingFilter(ConfigurationInterface* configuration, out_streams_(out_streams) { size_t item_size; - std::string default_input_item_type = "gr_complex"; std::string default_output_item_type = "gr_complex"; std::string default_dump_filename = "../data/input_filter.dat"; - + DLOG(INFO) << "role " << role_; input_item_type_ = config_->property(role_ + ".input_item_type", default_input_item_type); output_item_type_ = config_->property(role_ + ".output_item_type", default_output_item_type); dump_ = config_->property(role_ + ".dump", false); dump_filename_ = config_->property(role_ + ".dump_filename", default_dump_filename); - - double Pfa = config_->property(role_ + ".Pfa", 0.001); - + float default_pfa_ = 0.04; + float pfa = config_->property(role_ + ".pfa", default_pfa_); + 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_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) { item_size = sizeof(gr_complex); //output input_size_ = sizeof(gr_complex); //input - pulse_blanking_cc_ = make_pulse_blanking_cc(Pfa); + pulse_blanking_cc_ = make_pulse_blanking_cc(pfa, length_, n_segments_est, n_segments_reset); } else { diff --git a/src/algorithms/input_filter/adapters/pulse_blanking_filter.h b/src/algorithms/input_filter/adapters/pulse_blanking_filter.h index 4465fd960..18b040bcc 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 * * ------------------------------------------------------------------------- * @@ -39,9 +40,6 @@ class ConfigurationInterface; -/*! - * \brief TODO - */ class PulseBlankingFilter: public GNSSBlockInterface { public: diff --git a/src/algorithms/input_filter/gnuradio_blocks/CMakeLists.txt b/src/algorithms/input_filter/gnuradio_blocks/CMakeLists.txt index e4c2ba85d..0d169b019 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/input_filter/gnuradio_blocks/CMakeLists.txt @@ -20,6 +20,8 @@ set(INPUT_FILTER_GR_BLOCKS_SOURCES beamformer.cc pulse_blanking_cc.cc + notch_cc.cc + notch_lite_cc.cc ) include_directories( @@ -38,4 +40,4 @@ target_link_libraries(input_filter_gr_blocks ${GNURADIO_FILTER_LIBRARIES} ${VOL if(NOT VOLK_GNSSSDR_FOUND) add_dependencies(input_filter_gr_blocks volk_gnsssdr_module) -endif(NOT VOLK_GNSSSDR_FOUND) \ No newline at end of file +endif(NOT VOLK_GNSSSDR_FOUND) diff --git a/src/algorithms/input_filter/gnuradio_blocks/notch_cc.cc b/src/algorithms/input_filter/gnuradio_blocks/notch_cc.cc new file mode 100644 index 000000000..700ccfea8 --- /dev/null +++ b/src/algorithms/input_filter/gnuradio_blocks/notch_cc.cc @@ -0,0 +1,144 @@ +/*! + * \file notch_cc.cc + * \brief Implements a multi state notch filter algorithm + * \author Antonio Ramos (antonio.ramosdet(at)gmail.com) + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "notch_cc.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using google::LogMessage; + +notch_sptr make_notch_filter(float pfa, float p_c_factor, + int length_, int n_segments_est, int n_segments_reset) +{ + 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_, 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))) +{ + const int alignment_multiple = volk_get_alignment() / sizeof(gr_complex); + set_alignment(std::max(1, alignment_multiple)); + set_history(2); + this->pfa = pfa; + noise_pow_est = 0.0; + this->p_c_factor = gr_complex(p_c_factor , 0); + this->length_ = length_; //Set the number of samples per segment + set_output_multiple(length_); + filter_state_ = false; //Initial state of the filter + n_deg_fred = 2 * length_; //Number of dregrees of freedom + n_segments = 0; + 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)); + c_samples = static_cast(volk_malloc(length_ * sizeof(gr_complex), volk_get_alignment())); + angle_ = static_cast(volk_malloc(length_ * sizeof(float), volk_get_alignment())); + power_spect = static_cast(volk_malloc(length_ * sizeof(float), volk_get_alignment())); + last_out = gr_complex(0,0); +} + +Notch::~Notch() +{ + volk_free(c_samples); + volk_free(angle_); + volk_free(power_spect); +} + +int Notch::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]; + in++; + arma::cx_fvec signal_segment; + arma::cx_fvec signal_segment_fft; + while((index_out + length_) < noutput_items) + { + 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); + } + volk_32fc_x2_multiply_conjugate_32fc(c_samples, in, (in - 1), length_); + volk_32fc_s32f_atan2_32f(angle_, c_samples, ((float)1.0), length_); + for(int aux = 0; aux < length_; aux++) + { + z_0 = std::exp(gr_complex(0,1) * (*(angle_ + aux))); + *(out + aux) = *(in + aux) - z_0 * (*(in + aux - 1)) + p_c_factor * z_0 * last_out; + last_out = *(out + aux); + } + } + 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_; + } + consume_each(index_out); + return index_out; +} diff --git a/src/algorithms/input_filter/gnuradio_blocks/notch_cc.h b/src/algorithms/input_filter/gnuradio_blocks/notch_cc.h new file mode 100644 index 000000000..5be7dfda6 --- /dev/null +++ b/src/algorithms/input_filter/gnuradio_blocks/notch_cc.h @@ -0,0 +1,79 @@ +/*! + * \file notch_cc.h + * \brief Implements a notch filter algorithm + * \author Antonio Ramos (antonio.ramosdet(at)gmail.com) + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_NOTCH_H_ +#define GNSS_SDR_NOTCH_H_ + +#include +#include + +class Notch; + +typedef boost::shared_ptr notch_sptr; + +notch_sptr make_notch_filter(float pfa, float p_c_factor, + int length_, int n_segments_est, int n_segments_reset); + +/*! + * \brief This class implements a real-time software-defined multi state notch filter + */ + +class Notch : public gr::block +{ +private: + + float pfa; + float noise_pow_est; + float thres_; + int length_; + int n_deg_fred; + unsigned int n_segments; + unsigned int n_segments_est; + unsigned int n_segments_reset; + bool filter_state_; + gr_complex last_out; + gr_complex z_0; + gr_complex p_c_factor; + gr_complex* c_samples; + float* angle_; + float* power_spect; + +public: + + Notch(float pfa, float p_c_factor, int length_, int n_segments_est, int n_segments_reset); + + ~Notch(); + + int general_work (int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); +}; + +#endif //GNSS_SDR_NOTCH_H_ diff --git a/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.cc b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.cc new file mode 100644 index 000000000..a84ec6ae6 --- /dev/null +++ b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.cc @@ -0,0 +1,156 @@ +/*! + * \file notch_lite_cc.cc + * \brief Implements a multi state notch filter algorithm + * \author Antonio Ramos (antonio.ramosdet(at)gmail.com) + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "notch_lite_cc.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using google::LogMessage; + +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, pfa, length_, n_segments_est, n_segments_reset, n_segments_coeff)); +} + +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))) +{ + const int alignment_multiple = volk_get_alignment() / sizeof(gr_complex); + 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); + boost::math::chi_squared_distribution my_dist_(n_deg_fred); + thres_ = boost::math::quantile(boost::math::complement(my_dist_, pfa)); + 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())); + +} + +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]; + in++; + arma::cx_fvec signal_segment; + arma::cx_fvec signal_segment_fft; + while((index_out + length_) < noutput_items) + { + 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_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++) + { + *(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_; + } + 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 new file mode 100644 index 000000000..2151a459f --- /dev/null +++ b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h @@ -0,0 +1,82 @@ +/*! + * \file notch_lite_cc.h + * \brief Implements a notch filter ligth algorithm + * \author Antonio Ramos (antonio.ramosdet(at)gmail.com) + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_NOTCH_LITE_H_ +#define GNSS_SDR_NOTCH_LITE_H_ + +#include +#include + +class NotchLite; + +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 multi state notch filter ligth version + */ + +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_samples1; + gr_complex c_samples2; + float angle1; + float angle2; + float* power_spect; + +public: + + 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, + gr_vector_void_star &output_items); +}; + +#endif //GNSS_SDR_NOTCH_LITE_H_ 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 8e2cdcbcb..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) @@ -29,62 +29,90 @@ */ #include "pulse_blanking_cc.h" +#include #include #include #include #include -#include +#include +using google::LogMessage; -pulse_blanking_cc_sptr make_pulse_blanking_cc(double Pfa) +pulse_blanking_cc_sptr make_pulse_blanking_cc(float pfa, int length_, + int n_segments_est, int n_segments_reset) { - return pulse_blanking_cc_sptr(new pulse_blanking_cc(Pfa)); + return pulse_blanking_cc_sptr(new pulse_blanking_cc(pfa, length_, n_segments_est, n_segments_reset)); } - - -pulse_blanking_cc::pulse_blanking_cc(double Pfa) : gr::block("pulse_blanking_cc", +pulse_blanking_cc::pulse_blanking_cc(float pfa, int length_, int n_segments_est, int n_segments_reset) : gr::block("pulse_blanking_cc", gr::io_signature::make (1, 1, sizeof(gr_complex)), gr::io_signature::make (1, 1, sizeof(gr_complex))) { const int alignment_multiple = volk_get_alignment() / sizeof(gr_complex); set_alignment(std::max(1, alignment_multiple)); - d_Pfa = Pfa; + this->pfa = pfa; + this->length_ = length_; + set_output_multiple(length_); + last_filtered = false; + n_segments = 0; + this->n_segments_est = n_segments_est; + this->n_segments_reset = n_segments_reset; + noise_power_estimation = 0.0; + n_deg_fred = 2*length_; + boost::math::chi_squared_distribution my_dist_(n_deg_fred); + thres_ = boost::math::quantile(boost::math::complement(my_dist_, pfa)); + zeros_ = static_cast(volk_malloc(length_ * sizeof(gr_complex), volk_get_alignment())); + for (int aux = 0; aux < length_; aux++) + { + zeros_[aux] = gr_complex(0, 0); + } } +pulse_blanking_cc::~pulse_blanking_cc() +{ + volk_free(zeros_); +} int pulse_blanking_cc::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) { - const gr_complex *in = reinterpret_cast(input_items[0]); - gr_complex *out = reinterpret_cast(output_items[0]); - - // 1- (optional) Compute the input signal power estimation - //float mean; - //float stddev; - //volk_32f_stddev_and_mean_32f_x2(&stddev, &mean, in, noutput_items); - - float* magnitude; - magnitude = static_cast(volk_gnsssdr_malloc(noutput_items * sizeof(float), volk_gnsssdr_get_alignment())); - - float var; + gr_complex *in = (gr_complex *) input_items[0]; + gr_complex *out = (gr_complex *) output_items[0]; + float* magnitude = static_cast(volk_malloc(noutput_items * sizeof(float), volk_get_alignment())); volk_32fc_magnitude_squared_32f(magnitude, in, noutput_items); - volk_32f_accumulator_s32f(&var, magnitude, noutput_items); - var /= static_cast(noutput_items); - // compute pulse blanking threshold (Paper Borio 2016) - - float Th = sqrt(-2.0 * var * log10(d_Pfa)); - - //apply the pulse blanking - //todo: write volk kernel to optimize the blanking - memcpy(out,in, sizeof(gr_complex)*noutput_items); - for (int n = 0; n < noutput_items; n++) + int sample_index = 0; + float segment_energy; + while((sample_index + length_) < noutput_items) + { + volk_32f_accumulator_s32f(&segment_energy, (magnitude + sample_index), length_); + if((n_segments < n_segments_est) && (last_filtered == false)) { - if (std::abs(out[n]) > Th) - { - out[n] = gr_complex(0,0); - } + noise_power_estimation = (((float) n_segments) * noise_power_estimation + segment_energy / ((float)n_deg_fred)) / ((float)(n_segments + 1)); + memcpy(out, in, sizeof(gr_complex)*length_); } - consume_each(noutput_items); - return noutput_items; + else + { + if((segment_energy/noise_power_estimation) > thres_) + { + memcpy(out, zeros_, sizeof(gr_complex)*length_); + last_filtered = true; + } + else + { + memcpy(out, in, sizeof(gr_complex)*length_); + last_filtered = false; + if (n_segments > n_segments_reset) + { + n_segments = 0; + } + } + } + in+=length_; + out+=length_; + sample_index+=length_; + n_segments++; + } + volk_free(magnitude); + consume_each(sample_index); + return sample_index; } 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 8e23d39ad..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) @@ -38,22 +38,33 @@ class pulse_blanking_cc; typedef boost::shared_ptr pulse_blanking_cc_sptr; -pulse_blanking_cc_sptr make_pulse_blanking_cc(double Pfa); +pulse_blanking_cc_sptr make_pulse_blanking_cc(float pfa, int length_, int n_segments_est, int n_segments_reset); + -/*! - * \brief This class adapts a short (16-bits) interleaved sample stream - * into a std::complex stream - */ class pulse_blanking_cc : public gr::block { private: - friend pulse_blanking_cc_sptr make_pulse_blanking_cc(double Pfa); - double d_Pfa; + + int length_; + int n_segments; + int n_segments_est; + int n_segments_reset; + int n_deg_fred; + bool last_filtered; + float noise_power_estimation; + float thres_; + float pfa; + gr_complex* zeros_; + public: - pulse_blanking_cc(double Pfa); + + pulse_blanking_cc(float pfa, int length_, int n_segments_est, int n_segments_reset); + + ~pulse_blanking_cc(); int 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); + }; #endif diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 0bac3cbee..4daad0c79 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -65,6 +65,8 @@ #include "freq_xlating_fir_filter.h" #include "beamformer_filter.h" #include "pulse_blanking_filter.h" +#include "notch_filter.h" +#include "notch_filter_lite.h" #include "gps_l1_ca_pcps_acquisition.h" #include "gps_l2_m_pcps_acquisition.h" #include "gps_l1_ca_pcps_tong_acquisition.h" @@ -881,6 +883,18 @@ std::unique_ptr GNSSBlockFactory::GetBlock( out_streams)); block = std::move(block_); } + else if (implementation.compare("Notch_Filter") == 0) + { + std::unique_ptr block_(new NotchFilter(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } + else if (implementation.compare("Notch_Filter_Lite") == 0) + { + std::unique_ptr block_(new NotchFilterLite(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } // RESAMPLER -------------------------------------------------------------------