Merge branch 'next' of https://github.com/antonioramosdet/gnss-sdr into antonioramosdet-next

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(

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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "notch_filter.h"
+#include <string>
+#include <memory>
+#include <vector>
+#include <boost/lexical_cast.hpp>
+#include <gnuradio/blocks/file_sink.h>
+#include <glog/logging.h>
+#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_;
+}

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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_SDR_NOTCH_FILTER_H_
+#define GNSS_SDR_NOTCH_FILTER_H_
+
+#include <string>
+#include <vector>
+#include <gnuradio/blocks/file_sink.h>
+#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_

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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "notch_filter_lite.h"
+#include <string>
+#include <cmath>
+#include <memory>
+#include <vector>
+#include <boost/lexical_cast.hpp>
+#include <gnuradio/blocks/file_sink.h>
+#include <glog/logging.h>
+#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_;
+}

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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_SDR_NOTCH_FILTER_LITE_H_
+#define GNSS_SDR_NOTCH_FILTER_LITE_H_
+
+#include <string>
+#include <vector>
+#include <gnuradio/blocks/file_sink.h>
+#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_

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
         {

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:

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)
+endif(NOT VOLK_GNSSSDR_FOUND)

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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "notch_cc.h"
+#include <boost/math/distributions/chi_squared.hpp>
+#include <cmath>
+#include <complex>
+#include <cstdio>
+#include <cstring>
+#include <gnuradio/io_signature.h>
+#include <volk/volk.h>
+#include <iostream>
+#include <glog/logging.h>
+#include <armadillo>
+
+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<float> my_dist_(n_deg_fred);
+    thres_ = boost::math::quantile(boost::math::complement(my_dist_, pfa));
+    c_samples = static_cast<gr_complex *>(volk_malloc(length_ * sizeof(gr_complex), volk_get_alignment()));
+    angle_ = static_cast<float *>(volk_malloc(length_ * sizeof(float), volk_get_alignment()));
+    power_spect = static_cast<float *>(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;
+}

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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_SDR_NOTCH_H_
+#define GNSS_SDR_NOTCH_H_
+
+#include <boost/shared_ptr.hpp>
+#include <gnuradio/block.h>
+
+class Notch;
+
+typedef boost::shared_ptr<Notch> 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_

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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "notch_lite_cc.h"
+#include <cmath>
+#include <complex>
+#include <cstdio>
+#include <cstring>
+#include <gnuradio/io_signature.h>
+#include <volk/volk.h>
+#include <iostream>
+#include <glog/logging.h>
+#include <boost/math/distributions/chi_squared.hpp>
+#include <armadillo>
+
+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<float> 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<float *>(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;
+}

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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_SDR_NOTCH_LITE_H_
+#define GNSS_SDR_NOTCH_LITE_H_
+
+#include <boost/shared_ptr.hpp>
+#include <gnuradio/block.h>
+
+class NotchLite;
+
+typedef boost::shared_ptr<NotchLite> 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_

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 <boost/math/distributions/chi_squared.hpp>
 #include <cmath>
 #include <complex>
 #include <gnuradio/io_signature.h>
 #include <volk/volk.h>
-#include <volk_gnsssdr/volk_gnsssdr.h>
+#include <glog/logging.h>
 
+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<float> my_dist_(n_deg_fred);
+    thres_ = boost::math::quantile(boost::math::complement(my_dist_, pfa));
+    zeros_ = static_cast<gr_complex *>(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<const gr_complex *>(input_items[0]);
-    gr_complex *out = reinterpret_cast<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<float*>(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<float *>(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<float>(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;    
 }

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> 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<short> 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

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<GNSSBlockInterface> GNSSBlockFactory::GetBlock(
                     out_streams));
             block = std::move(block_);
         }
+    else if (implementation.compare("Notch_Filter") == 0)
+        {
+            std::unique_ptr<GNSSBlockInterface> 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<GNSSBlockInterface> block_(new NotchFilterLite(configuration.get(), role, in_streams,
+                    out_streams));
+            block = std::move(block_);
+        }
 
 
     // RESAMPLER -------------------------------------------------------------------