Merge branch 'smooth-cn0' into next

Add a CN0 smoother to reduce the variance of the estimation

src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc

@@ -439,7 +439,11 @@ 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();
+    if (d_code_period > 0.0)
+        {
+            d_cn0_smoother.set_samples_for_initialization(200 / static_cast<int>(d_code_period * 1000.0));
+        }
     d_acquisition_gnss_synchro = nullptr;
     d_channel = 0;
     d_acq_code_phase_samples = 0.0;
@@ -848,9 +852,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);
+    float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer, trk_parameters.cn0_samples, static_cast<float>(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
@@ -1589,6 +1596,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)
                 d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast<double>(samples_offset);
                 d_state = 2;
                 d_sample_counter += samples_offset;  // count for the processed samples
+                d_cn0_smoother.reset();
 
                 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

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/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc

@@ -350,6 +350,11 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga &
     d_Prompt_buffer = new gr_complex[trk_parameters.cn0_samples];
     d_carrier_lock_test = 1.0;
     d_CN0_SNV_dB_Hz = 0.0;
+    d_cn0_smoother = Exponential_Smoother();
+    if (d_code_period > 0.0)
+        {
+            d_cn0_smoother.set_samples_for_initialization(200 / static_cast<int>(d_code_period * 1000.0));
+        }
     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()));
@@ -582,7 +587,8 @@ bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integra
         {
             d_cn0_estimation_counter = 0;
             // Code lock indicator
-            d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, trk_parameters.cn0_samples, coh_integration_time_s);
+            float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer, trk_parameters.cn0_samples, static_cast<float>(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
@@ -1437,6 +1443,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un
                 int32_t samples_offset = round(d_acq_code_phase_samples);
                 d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast<double>(samples_offset);
                 d_state = 2;
+                d_cn0_smoother.reset();
 
                 // DEBUG OUTPUT
                 std::cout << "Tracking of " << systemName << " " << signal_pretty_name << " signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl;

src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h

@@ -33,6 +33,7 @@
 #define GNSS_SDR_DLL_PLL_VEML_TRACKING_FPGA_H
 
 #include "dll_pll_conf_fpga.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>
@@ -201,6 +202,7 @@ private:
     boost::circular_buffer<gr_complex> d_Prompt_circular_buffer;
     //std::deque<gr_complex> d_Prompt_buffer_deque;
     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,138 @@
+/*!
+ * \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 <iterator>
+#include <numeric>
+
+Exponential_Smoother::Exponential_Smoother()
+{
+    alpha_ = 0.001;
+    old_value_ = 0.0;
+    one_minus_alpha_ = 1.0 - alpha_;
+    samples_for_initialization_ = 200;
+    initializing_ = true;
+    init_counter_ = 0;
+    min_value_ = 25.0;
+    offset_ = 12.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.0f) / 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.001
+    void set_samples_for_initialization(int num_samples); //!< Number of samples averaged for initialization. Default value: 200
+    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_

src/algorithms/tracking/libs/lock_detectors.cc

@@ -65,23 +65,23 @@
  * where \f$T_{int}\f$ is the coherent integration time, in seconds.
  *
  */
-float cn0_svn_estimator(const gr_complex* Prompt_buffer, int length, double coh_integration_time_s)
+float cn0_svn_estimator(const gr_complex* Prompt_buffer, int length, float coh_integration_time_s)
 {
-    double SNR = 0.0;
-    double SNR_dB_Hz = 0.0;
-    double Psig = 0.0;
-    double Ptot = 0.0;
+    float SNR = 0.0;
+    float SNR_dB_Hz = 0.0;
+    float Psig = 0.0;
+    float Ptot = 0.0;
     for (int i = 0; i < length; i++)
         {
-            Psig += std::abs(static_cast<double>(Prompt_buffer[i].real()));
-            Ptot += static_cast<double>(Prompt_buffer[i].imag()) * static_cast<double>(Prompt_buffer[i].imag()) + static_cast<double>(Prompt_buffer[i].real()) * static_cast<double>(Prompt_buffer[i].real());
+            Psig += std::abs(Prompt_buffer[i].real());
+            Ptot += Prompt_buffer[i].imag() * Prompt_buffer[i].imag() + Prompt_buffer[i].real() * Prompt_buffer[i].real();
         }
-    Psig /= static_cast<double>(length);
+    Psig /= static_cast<float>(length);
     Psig = Psig * Psig;
-    Ptot /= static_cast<double>(length);
+    Ptot /= static_cast<float>(length);
     SNR = Psig / (Ptot - Psig);
-    SNR_dB_Hz = 10.0 * log10(SNR) - 10.0 * log10(coh_integration_time_s);
-    return static_cast<float>(SNR_dB_Hz);
+    SNR_dB_Hz = 10.0 * std::log10(SNR) - 10.0 * std::log10(coh_integration_time_s);
+    return SNR_dB_Hz;
 }
 
 

src/algorithms/tracking/libs/lock_detectors.h

@@ -72,7 +72,7 @@
  * IEEE 10th International Symposium on Spread Spectrum Techniques and
  * Applications, pp.28-30, August 2008.
  */
-float cn0_svn_estimator(const gr_complex* Prompt_buffer, int length, double coh_integration_time_s);
+float cn0_svn_estimator(const gr_complex* Prompt_buffer, int length, float coh_integration_time_s);
 
 
 /*! \brief A carrier lock detector