Add a simple CN0 smoother

src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc

@@ -439,6 +439,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl
     d_carrier_lock_fail_counter = 0;
     d_carrier_lock_threshold = trk_parameters.carrier_lock_th;
     d_Prompt_Data = static_cast<gr_complex *>(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment()));
+    d_cn0_smoother = Exponential_Smoother();
 
     d_acquisition_gnss_synchro = nullptr;
     d_channel = 0;
@@ -848,9 +849,12 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status(double coh_integration_
             d_cn0_estimation_counter++;
             return true;
         }
-    d_cn0_estimation_counter = 0;
+
+    d_Prompt_buffer[d_cn0_estimation_counter % trk_parameters.cn0_samples] = d_P_accu;
+    d_cn0_estimation_counter++;
     // Code lock indicator
-    d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, trk_parameters.cn0_samples, coh_integration_time_s);
+    double d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer, trk_parameters.cn0_samples, coh_integration_time_s);
+    d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw);
     // Carrier lock indicator
     d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, trk_parameters.cn0_samples);
     // Loss of lock detection
@@ -1593,7 +1597,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)
                 DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)";
                 DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz
                            << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples;
-
+                d_cn0_smoother.reset();
                 consume_each(samples_offset);  // shift input to perform alignment with local replica
                 return 0;
             }

src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h

@@ -34,6 +34,7 @@
 
 #include "cpu_multicorrelator_real_codes.h"
 #include "dll_pll_conf.h"
+#include "exponential_smoother.h"
 #include "tracking_FLL_PLL_filter.h"  // for PLL/FLL filter
 #include "tracking_loop_filter.h"     // for DLL filter
 #include <boost/circular_buffer.hpp>
@@ -197,6 +198,7 @@ private:
     double d_carrier_lock_threshold;
     boost::circular_buffer<gr_complex> d_Prompt_circular_buffer;
     gr_complex *d_Prompt_buffer;
+    Exponential_Smoother d_cn0_smoother;
 
     // file dump
     std::ofstream d_dump_file;

src/algorithms/tracking/libs/CMakeLists.txt

@@ -43,6 +43,7 @@ set(TRACKING_LIB_SOURCES
     tracking_loop_filter.cc
     dll_pll_conf.cc
     bayesian_estimation.cc
+    exponential_smoother.cc
 )
 
 set(TRACKING_LIB_HEADERS
@@ -59,6 +60,7 @@ set(TRACKING_LIB_HEADERS
     tracking_loop_filter.h
     dll_pll_conf.h
     bayesian_estimation.h
+    exponential_smoother.h
 )
 
 if(ENABLE_FPGA)

src/algorithms/tracking/libs/exponential_smoother.cc

@@ -0,0 +1,139 @@
+/*!
+ * \file exponential_smoother.cc
+ * \brief Class that implements an exponential smoother
+ * \authors Carles Fernandez, 2019 cfernandez@cttc.es
+ *
+ * Class that implements a first-order exponential smoother.
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2019  (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 <https://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "exponential_smoother.h"
+#include <iostream>  ///////////////
+#include <iterator>
+#include <numeric>
+
+Exponential_Smoother::Exponential_Smoother()
+{
+    alpha_ = 0.001;
+    old_value_ = 0.0;
+    one_minus_alpha_ = 1.0 - alpha_;
+    samples_for_initialization_ = 500;
+    initializing_ = true;
+    init_counter_ = 0;
+    min_value_ = 25;
+    offset_ = 9.0;
+    init_buffer_.reserve(samples_for_initialization_);
+}
+
+
+Exponential_Smoother::~Exponential_Smoother() = default;
+
+
+void Exponential_Smoother::set_alpha(float alpha)
+{
+    alpha_ = alpha;
+    if (alpha_ < 0)
+        {
+            alpha_ = 0;
+        }
+    if (alpha_ > 1)
+        {
+            alpha_ = 1;
+        }
+    one_minus_alpha_ = 1.0 - alpha_;
+}
+
+
+void Exponential_Smoother::set_offset(float offset)
+{
+    offset_ = offset;
+}
+
+
+void Exponential_Smoother::set_samples_for_initialization(int num_samples)
+{
+    int ns = num_samples;
+    if (ns <= 0)
+        {
+            ns = 1;
+        }
+    samples_for_initialization_ = num_samples;
+    init_buffer_.reserve(samples_for_initialization_);
+}
+
+
+void Exponential_Smoother::reset()
+{
+    initializing_ = true;
+    init_counter_ = 0;
+    init_buffer_.clear();
+}
+
+
+void Exponential_Smoother::set_min_value(float value)
+{
+    min_value_ = value;
+}
+
+
+double Exponential_Smoother::smooth(double raw)
+{
+    float raw_f = static_cast<float>(raw);
+    double smooth_d = static_cast<double>((this)->smooth(raw_f));
+    return smooth_d;
+}
+
+
+float Exponential_Smoother::smooth(float raw)
+{
+    float smoothed_value;
+    if (initializing_ == true)
+        {
+            init_counter_++;
+            smoothed_value = raw;
+            init_buffer_.push_back(smoothed_value);
+            if (init_counter_ == samples_for_initialization_)
+                {
+                    old_value_ = std::accumulate(std::begin(init_buffer_), std::end(init_buffer_), 0.0) / static_cast<float>(init_buffer_.size());
+                    if (old_value_ < (min_value_ + offset_))
+                        {
+                            // flush buffer and start again
+                            init_counter_ = 0;
+                            init_buffer_.clear();
+                        }
+                    else
+                        {
+                            initializing_ = false;
+                        }
+                }
+        }
+    else
+        {
+            smoothed_value = alpha_ * raw + one_minus_alpha_ * old_value_;
+            old_value_ = smoothed_value;
+        }
+    return smoothed_value;
+}

src/algorithms/tracking/libs/exponential_smoother.h

@@ -0,0 +1,71 @@
+/*!
+ * \file exponential_smoother.h
+ * \brief Class that implements an exponential smoother
+ * \authors Carles Fernandez, 2019 cfernandez@cttc.es
+ *
+ * Class that implements a first-order exponential smoother.
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2019  (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 <https://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+
+#ifndef GNSS_SDR_EXPONENTIAL_SMOOTHER_H_
+#define GNSS_SDR_EXPONENTIAL_SMOOTHER_H_
+
+#include <vector>
+
+/*! \brief
+ * Class that implements a first-order exponential smoother.
+ *
+ * smoothed_value[k] = alpha * raw + (1-alpha) * smoothed_value[k-1]
+ *
+ * The length of the initialization can be controlled with
+ * set_samples_for_initialization(int num_samples)
+ */
+class Exponential_Smoother
+{
+public:
+    Exponential_Smoother();   //!< Constructor
+    ~Exponential_Smoother();  //!< Destructor
+    void set_alpha(float alpha); //!< 0 < alpha < 1. The higher, the most responsive, but more variance. Default value: 0.0001
+    void set_samples_for_initialization(int num_samples); //!< Number of samples averaged for initialization. Default value:
+    void reset();
+    void set_min_value(float value);
+    void set_offset(float offset);
+    float smooth(float raw);
+    double smooth(double raw);
+private:
+    float alpha_;  // takes value 0.0001 if not set
+    int samples_for_initialization_;
+    float one_minus_alpha_;
+    float old_value_;
+    float min_value_;
+    float offset_;
+    bool initializing_;
+    int init_counter_;
+    std::vector<float> init_buffer_;
+};
+
+#endif  // GNSS_SDR_EXPONENTIAL_SMOOTHER_H_