diff --git a/src/algorithms/input_filter/adapters/CMakeLists.txt b/src/algorithms/input_filter/adapters/CMakeLists.txt
index daafaf401..2cee74880 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..a69c0c9c0
--- /dev/null
+++ b/src/algorithms/input_filter/adapters/notch_filter.cc
@@ -0,0 +1,120 @@
+/*!
+ * \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;
+ 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_);
+ if (item_type_.compare("gr_complex") == 0)
+ {
+ item_size_ = sizeof(gr_complex);
+ notch_filter_ = make_notch_filter(pfa, p_c_factor, length_);
+ 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..09ac10e99
--- /dev/null
+++ b/src/algorithms/input_filter/adapters/notch_filter.h
@@ -0,0 +1,84 @@
+/*!
+ * \file notch_filter.h
+ * \brief
+ * \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..ec2ac636e
--- /dev/null
+++ b/src/algorithms/input_filter/adapters/notch_filter_lite.cc
@@ -0,0 +1,114 @@
+/*!
+ * \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 "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;
+ 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);
+ if (item_type_.compare("gr_complex") == 0)
+ {
+ item_size_ = sizeof(gr_complex);
+ notch_filter_lite_ = make_notch_filter_lite(p_c_factor);
+ 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..03796f319
--- /dev/null
+++ b/src/algorithms/input_filter/adapters/notch_filter_lite.h
@@ -0,0 +1,84 @@
+/*!
+ * \file notch_filter_lite.h
+ * \brief
+ * \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 4bca7a799..fed7205cc 100644
--- a/src/algorithms/input_filter/adapters/pulse_blanking_filter.cc
+++ b/src/algorithms/input_filter/adapters/pulse_blanking_filter.cc
@@ -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_estimation", 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 ce812556d..041ba2251 100644
--- a/src/algorithms/input_filter/adapters/pulse_blanking_filter.h
+++ b/src/algorithms/input_filter/adapters/pulse_blanking_filter.h
@@ -39,9 +39,6 @@
class ConfigurationInterface;
-/*!
- * \brief TODO
- */
class PulseBlankingFilter: public GNSSBlockInterface
{
public:
@@ -76,8 +73,6 @@ private:
std::string input_item_type_;
size_t input_size_;
std::string output_item_type_;
- double intermediate_freq_;
- double sampling_freq_;
std::string role_;
unsigned int in_streams_;
unsigned int out_streams_;
diff --git a/src/algorithms/input_filter/gnuradio_blocks/CMakeLists.txt b/src/algorithms/input_filter/gnuradio_blocks/CMakeLists.txt
index e2c3a3674..b0ce4a55e 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..007b3d9b3
--- /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_)
+{
+ return notch_sptr(new Notch(pfa, p_c_factor, length_));
+}
+
+Notch::Notch(float pfa, float p_c_factor, int length_) : 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;
+ 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
+ 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..faf7c5d0c
--- /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_);
+
+/*!
+ * \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_);
+
+ ~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..4b21bf33e
--- /dev/null
+++ b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.cc
@@ -0,0 +1,80 @@
+/*!
+ * \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
+
+using google::LogMessage;
+
+notch_lite_sptr make_notch_filter_lite(float p_c_factor)
+{
+ return notch_lite_sptr(new NotchLite(p_c_factor));
+}
+
+NotchLite::NotchLite(float p_c_factor) : 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);
+ z_0 = gr_complex(0 , 0);
+ last_out = gr_complex(0,0);
+}
+
+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)
+{
+ 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++)
+ {
+ 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);
+ }
+ volk_free(c_samples);
+ volk_free(angle_);
+ consume_each(noutput_items);
+ return noutput_items;
+}
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..16f56286d
--- /dev/null
+++ b/src/algorithms/input_filter/gnuradio_blocks/notch_lite_cc.h
@@ -0,0 +1,66 @@
+/*!
+ * \file notch_lite_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_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);
+
+/*!
+ * \brief This class implements a real-time software-defined single state notch filter
+ */
+
+class NotchLite : public gr::block
+{
+private:
+
+ gr_complex last_out;
+ gr_complex z_0;
+ gr_complex p_c_factor;
+ gr_complex* c_samples;
+ float* angle_;
+
+public:
+
+ NotchLite(float p_c_factor);
+
+ 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 f2ec0f198..a744f8747 100644
--- a/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.cc
+++ b/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.cc
@@ -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 = (const gr_complex *) input_items[0];
+ gr_complex *in = (gr_complex *) input_items[0];
gr_complex *out = (gr_complex *) 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;
+ 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..a77cf4deb 100644
--- a/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h
+++ b/src/algorithms/input_filter/gnuradio_blocks/pulse_blanking_cc.h
@@ -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 0e85d4f2d..6efe76db5 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_multithread_acquisition.h"
@@ -882,6 +884,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 -------------------------------------------------------------------