diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 4ccb3222f..57c19c994 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/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(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(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(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(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 diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index 43d95f3f6..1ccb44667 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/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 @@ -197,6 +198,7 @@ private: double d_carrier_lock_threshold; boost::circular_buffer d_Prompt_circular_buffer; gr_complex *d_Prompt_buffer; + Exponential_Smoother d_cn0_smoother; // file dump std::ofstream d_dump_file; diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc index 185281684..43cc30e7a 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc +++ b/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(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(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(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(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; diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h index 388775a7e..0cb708323 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h +++ b/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 @@ -201,6 +202,7 @@ private: boost::circular_buffer d_Prompt_circular_buffer; //std::deque d_Prompt_buffer_deque; gr_complex *d_Prompt_buffer; + Exponential_Smoother d_cn0_smoother; // file dump std::ofstream d_dump_file; diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index 7a7951645..763ce9b07 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/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) diff --git a/src/algorithms/tracking/libs/exponential_smoother.cc b/src/algorithms/tracking/libs/exponential_smoother.cc new file mode 100644 index 000000000..e05ff9c9b --- /dev/null +++ b/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 . + * + * ------------------------------------------------------------------------- + */ + +#include "exponential_smoother.h" +#include +#include + +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(raw); + double smooth_d = static_cast((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(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; +} diff --git a/src/algorithms/tracking/libs/exponential_smoother.h b/src/algorithms/tracking/libs/exponential_smoother.h new file mode 100644 index 000000000..f8ff21182 --- /dev/null +++ b/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 . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_EXPONENTIAL_SMOOTHER_H_ +#define GNSS_SDR_EXPONENTIAL_SMOOTHER_H_ + +#include + +/*! \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 init_buffer_; +}; + +#endif // GNSS_SDR_EXPONENTIAL_SMOOTHER_H_ diff --git a/src/algorithms/tracking/libs/lock_detectors.cc b/src/algorithms/tracking/libs/lock_detectors.cc index b66e3a15d..6633b893a 100644 --- a/src/algorithms/tracking/libs/lock_detectors.cc +++ b/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(Prompt_buffer[i].real())); - Ptot += static_cast(Prompt_buffer[i].imag()) * static_cast(Prompt_buffer[i].imag()) + static_cast(Prompt_buffer[i].real()) * static_cast(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(length); + Psig /= static_cast(length); Psig = Psig * Psig; - Ptot /= static_cast(length); + Ptot /= static_cast(length); SNR = Psig / (Ptot - Psig); - SNR_dB_Hz = 10.0 * log10(SNR) - 10.0 * log10(coh_integration_time_s); - return static_cast(SNR_dB_Hz); + SNR_dB_Hz = 10.0 * std::log10(SNR) - 10.0 * std::log10(coh_integration_time_s); + return SNR_dB_Hz; } diff --git a/src/algorithms/tracking/libs/lock_detectors.h b/src/algorithms/tracking/libs/lock_detectors.h index 90dc90fcb..81d54b651 100644 --- a/src/algorithms/tracking/libs/lock_detectors.h +++ b/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