From a8c76ce0b428cb3f192e8c129f88928ca4c53de8 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 27 Jul 2026 20:00:58 +0200 Subject: [PATCH] Improve CN0 estimation and lock detection in tracking blocks --- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 10 +++- .../dll_pll_veml_tracking_fpga.cc | 10 +++- .../tracking/gnuradio_blocks/kf_tracking.cc | 10 +++- .../tracking/libs/exponential_smoother.cc | 6 +- .../tracking/libs/exponential_smoother.h | 8 +++ .../tracking/libs/lock_detectors.cc | 55 ++++++++++--------- 6 files changed, 65 insertions(+), 34 deletions(-) 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 c0b47db0b..653f6158d 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -680,10 +680,12 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) d_Prompt_Data = volk_gnsssdr::vector(1); d_cn0_smoother = Exponential_Smoother(); d_cn0_smoother.set_alpha(d_trk_parameters.cn0_smoother_alpha); + d_cn0_smoother.set_min_value(static_cast(d_trk_parameters.cn0_min)); + d_cn0_smoother.set_offset(0.0); if (d_code_period > 0.0) { - d_cn0_smoother.set_samples_for_initialization(d_trk_parameters.cn0_smoother_samples / static_cast(d_code_period * 1000.0)); + d_cn0_smoother.set_samples_for_initialization(d_trk_parameters.cn0_smoother_samples / std::max(1, static_cast(d_code_period * 1000.0))); } d_carrier_lock_test_smoother = Exponential_Smoother(); @@ -1198,7 +1200,7 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status(double coh_integration_ const float d_CN0_SNV_dB_Hz_raw = cn0_m2m4_estimator(d_Prompt_buffer.data(), d_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 = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer.data(), 1)); + d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(&d_P_accu, 1)); // Loss of lock detection if (!d_pull_in_transitory) { @@ -2142,6 +2144,10 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) // UPDATE INTEGRATION TIME d_extend_correlation_symbols_count = 0; d_current_correlation_time_s = static_cast(d_extend_correlation_symbols) * static_cast(d_code_period); + // the CN0 estimation buffer must not mix prompts from different integration times + d_cn0_estimation_counter = 0; + // keep the smoother initialization at the same data-time budget under the new update period + d_cn0_smoother.set_samples_for_initialization(d_trk_parameters.cn0_smoother_samples / std::max(1, static_cast(d_current_correlation_time_s * 1000.0))); d_state = 3; // next state is the extended correlator integrator LOG(INFO) << "Enabled " << d_extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " << d_channel 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 c198bba67..ca80d1234 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 @@ -415,9 +415,11 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga & d_Prompt_Data = volk_gnsssdr::vector(1); d_cn0_smoother = Exponential_Smoother(); d_cn0_smoother.set_alpha(d_trk_parameters.cn0_smoother_alpha); + d_cn0_smoother.set_min_value(static_cast(d_trk_parameters.cn0_min)); + d_cn0_smoother.set_offset(0.0); if (d_code_period > 0.0) { - d_cn0_smoother.set_samples_for_initialization(d_trk_parameters.cn0_smoother_samples / static_cast(d_code_period * 1000.0)); + d_cn0_smoother.set_samples_for_initialization(d_trk_parameters.cn0_smoother_samples / std::max(1, static_cast(d_code_period * 1000.0))); } d_carrier_lock_test_smoother = Exponential_Smoother(); @@ -629,7 +631,7 @@ bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integra const float d_CN0_SNV_dB_Hz_raw = cn0_m2m4_estimator(d_Prompt_buffer.data(), d_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 = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer.data(), 1)); + d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(&d_P_accu, 1)); // Loss of lock detection if (!d_pull_in_transitory) { @@ -1719,6 +1721,10 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un // update integration time d_extend_correlation_symbols_count = 0; d_current_correlation_time_s = static_cast(d_trk_parameters.extend_correlation_symbols) * static_cast(d_code_period); + // the CN0 estimation buffer must not mix prompts from different integration times + d_cn0_estimation_counter = 0; + // keep the smoother initialization at the same data-time budget under the new update period + d_cn0_smoother.set_samples_for_initialization(d_trk_parameters.cn0_smoother_samples / std::max(1, static_cast(d_current_correlation_time_s * 1000.0))); if (d_extended_correlation_in_fpga) { diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc index 2fbf65163..45c1a0399 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc @@ -537,10 +537,12 @@ kf_tracking::kf_tracking(const Kf_Conf &conf_) d_Prompt_Data = volk_gnsssdr::vector(1); d_cn0_smoother = Exponential_Smoother(); d_cn0_smoother.set_alpha(d_trk_parameters.cn0_smoother_alpha); + d_cn0_smoother.set_min_value(static_cast(d_trk_parameters.cn0_min)); + d_cn0_smoother.set_offset(0.0); if (d_code_period > 0.0) { - d_cn0_smoother.set_samples_for_initialization(d_trk_parameters.cn0_smoother_samples / static_cast(d_code_period * 1000.0)); + d_cn0_smoother.set_samples_for_initialization(d_trk_parameters.cn0_smoother_samples / std::max(1, static_cast(d_code_period * 1000.0))); } d_carrier_lock_test_smoother = Exponential_Smoother(); @@ -1057,7 +1059,7 @@ bool kf_tracking::cn0_and_tracking_lock_status(double coh_integration_time_s) const float d_CN0_SNV_dB_Hz_raw = cn0_m2m4_estimator(d_Prompt_buffer.data(), d_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 = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer.data(), 1)); + d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(&d_P_accu, 1)); // Loss of lock detection if (!d_pull_in_transitory) { @@ -1874,6 +1876,10 @@ int kf_tracking::general_work(int noutput_items __attribute__((unused)), gr_vect // UPDATE INTEGRATION TIME d_extend_correlation_symbols_count = 0; d_current_correlation_time_s = static_cast(d_trk_parameters.extend_correlation_symbols) * static_cast(d_code_period); + // the CN0 estimation buffer must not mix prompts from different integration times + d_cn0_estimation_counter = 0; + // keep the smoother initialization at the same data-time budget under the new update period + d_cn0_smoother.set_samples_for_initialization(d_trk_parameters.cn0_smoother_samples / std::max(1, static_cast(d_current_correlation_time_s * 1000.0))); d_state = 3; // next state is the extended correlator integrator LOG(INFO) << "Enabled " << d_trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " << d_channel diff --git a/src/algorithms/tracking/libs/exponential_smoother.cc b/src/algorithms/tracking/libs/exponential_smoother.cc index 63294bb2c..53fe43b02 100644 --- a/src/algorithms/tracking/libs/exponential_smoother.cc +++ b/src/algorithms/tracking/libs/exponential_smoother.cc @@ -63,6 +63,7 @@ void Exponential_Smoother::reset() { initializing_ = true; init_counter_ = 0; + init_attempts_ = 0; init_buffer_.clear(); } @@ -89,12 +90,13 @@ float Exponential_Smoother::smooth(float raw) init_counter_++; smoothed_value = raw; init_buffer_.push_back(smoothed_value); - if (init_counter_ == samples_for_initialization_) + 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_)) + if (old_value_ < (min_value_ + offset_) && init_attempts_ < max_init_attempts_) { // flush buffer and start again + init_attempts_++; init_counter_ = 0; init_buffer_.clear(); } diff --git a/src/algorithms/tracking/libs/exponential_smoother.h b/src/algorithms/tracking/libs/exponential_smoother.h index 9804c8697..1cadc2cff 100644 --- a/src/algorithms/tracking/libs/exponential_smoother.h +++ b/src/algorithms/tracking/libs/exponential_smoother.h @@ -35,6 +35,12 @@ * * The length of the initialization can be controlled with * set_samples_for_initialization(int num_samples) + * + * If the average of an initialization window falls below + * min_value + offset, the window is discarded and initialization + * starts again, up to a maximum number of attempts; after that, the + * smoother engages with the last window average so that weak but valid + * signals still get smoothed. */ class Exponential_Smoother { @@ -62,6 +68,8 @@ private: float offset_{12.0}; int samples_for_initialization_{200}; int init_counter_{0}; + int init_attempts_{0}; + int max_init_attempts_{3}; bool initializing_{true}; }; diff --git a/src/algorithms/tracking/libs/lock_detectors.cc b/src/algorithms/tracking/libs/lock_detectors.cc index 0399ea531..4878e377c 100644 --- a/src/algorithms/tracking/libs/lock_detectors.cc +++ b/src/algorithms/tracking/libs/lock_detectors.cc @@ -53,30 +53,31 @@ */ float cn0_svn_estimator(const gr_complex* Prompt_buffer, int length, float coh_integration_time_s) { - float SNR = 0.0; - float SNR_dB_Hz = 0.0; - float Psig = 0.0; - float Ptot = 0.0; + // Accumulation in double precision: the Ptot - Psig subtraction below + // is a catastrophic cancellation at high C/N0 + double SNR = 0.0; + double Psig = 0.0; + double Ptot = 0.0; if (length == 0 || coh_integration_time_s == 0.0) { return -100.0; } for (int i = 0; i < length; i++) { - Psig += std::fabs(Prompt_buffer[i].real()); - Ptot += Prompt_buffer[i].imag() * Prompt_buffer[i].imag() + Prompt_buffer[i].real() * Prompt_buffer[i].real(); + Psig += std::fabs(static_cast(Prompt_buffer[i].real())); + Ptot += static_cast(Prompt_buffer[i].imag()) * Prompt_buffer[i].imag() + static_cast(Prompt_buffer[i].real()) * Prompt_buffer[i].real(); } - Psig /= static_cast(length); + Psig /= static_cast(length); Psig = Psig * Psig; - Ptot /= static_cast(length); - float aux = Ptot - Psig; - if (aux == 0.0) + Ptot /= static_cast(length); + const double aux = Ptot - Psig; + if (aux <= 0) { return -100.0; } SNR = Psig / aux; - SNR_dB_Hz = 10.0F * std::log10(SNR) - 10.0F * std::log10(coh_integration_time_s); - return SNR_dB_Hz; + const double SNR_dB_Hz = 10.0 * std::log10(SNR) - 10.0 * std::log10(static_cast(coh_integration_time_s)); + return static_cast(SNR_dB_Hz); } @@ -98,21 +99,23 @@ float cn0_svn_estimator(const gr_complex* Prompt_buffer, int length, float coh_i */ float cn0_m2m4_estimator(const gr_complex* Prompt_buffer, int length, float coh_integration_time_s) { - float SNR_aux = 0.0; - float SNR_dB_Hz = 0.0; - float Psig = 0.0; - float m_2 = 0.0; - float m_4 = 0.0; - float aux; - const auto n = static_cast(length); + // Moments are accumulated in double precision: |P|^4 overflows single + // precision for large correlator amplitudes, and the m_2 - sqrt(...) + // subtraction below is a catastrophic cancellation at high C/N0 + double SNR_aux = 0.0; + double Psig = 0.0; + double m_2 = 0.0; + double m_4 = 0.0; + double aux; + const auto n = static_cast(length); if (length == 0 || coh_integration_time_s == 0.0) { return -100.0; } for (int i = 0; i < length; i++) { - Psig += std::fabs(Prompt_buffer[i].real()); - aux = Prompt_buffer[i].imag() * Prompt_buffer[i].imag() + Prompt_buffer[i].real() * Prompt_buffer[i].real(); + Psig += std::fabs(static_cast(Prompt_buffer[i].real())); + aux = static_cast(Prompt_buffer[i].imag()) * Prompt_buffer[i].imag() + static_cast(Prompt_buffer[i].real()) * Prompt_buffer[i].real(); m_2 += aux; m_4 += (aux * aux); } @@ -120,8 +123,8 @@ float cn0_m2m4_estimator(const gr_complex* Prompt_buffer, int length, float coh_ Psig = Psig * Psig; m_2 /= n; m_4 /= n; - aux = std::sqrt(2.0F * m_2 * m_2 - m_4); - float denominator; + aux = std::sqrt(2.0 * m_2 * m_2 - m_4); + double denominator; if (std::isnan(aux)) { denominator = m_2 - Psig; @@ -141,13 +144,13 @@ float cn0_m2m4_estimator(const gr_complex* Prompt_buffer, int length, float coh_ SNR_aux = aux / denominator; } - if (SNR_aux == 0) + if (SNR_aux <= 0) { return -100.0; } - SNR_dB_Hz = 10.0F * std::log10(SNR_aux) - 10.0F * std::log10(coh_integration_time_s); + const double SNR_dB_Hz = 10.0 * std::log10(SNR_aux) - 10.0 * std::log10(static_cast(coh_integration_time_s)); - return SNR_dB_Hz; + return static_cast(SNR_dB_Hz); }