mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2026-10-12 02:31:51 +00:00
Code Issues Releases Wiki Activity

Improve CN0 estimation and lock detection in tracking blocks

Browse Source
This commit is contained in:
Carles Fernandez
2026-07-27 20:00:58 +02:00
parent 5451c100b6
commit a8c76ce0b4
6 changed files with 65 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc
+8 -2
View File
@@ -680,10 +680,12 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_)
d_Prompt_Data = volk_gnsssdr::vector<gr_complex>(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<float>(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<int>(d_code_period * 1000.0));
d_cn0_smoother.set_samples_for_initialization(d_trk_parameters.cn0_smoother_samples / std::max(1, static_cast<int>(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<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 = 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<float>(d_extend_correlation_symbols) * static_cast<float>(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<int>(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<int32_t>(d_code_period * 1000.0) << " ms extended correlator in channel "
<< d_channel
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc
+8 -2
View File
@@ -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<gr_complex>(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<float>(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<int>(d_code_period * 1000.0));
d_cn0_smoother.set_samples_for_initialization(d_trk_parameters.cn0_smoother_samples / std::max(1, static_cast<int>(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<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 = 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<float>(d_trk_parameters.extend_correlation_symbols) * static_cast<float>(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<int>(d_current_correlation_time_s * 1000.0)));
if (d_extended_correlation_in_fpga)
{
src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc
+8 -2
View File
@@ -537,10 +537,12 @@ kf_tracking::kf_tracking(const Kf_Conf &conf_)
d_Prompt_Data = volk_gnsssdr::vector<gr_complex>(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<float>(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<int>(d_code_period * 1000.0));
d_cn0_smoother.set_samples_for_initialization(d_trk_parameters.cn0_smoother_samples / std::max(1, static_cast<int>(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<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 = 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<float>(d_trk_parameters.extend_correlation_symbols) * static_cast<float>(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<int>(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<int32_t>(d_code_period * 1000.0) << " ms extended correlator in channel "
<< d_channel
src/algorithms/tracking/libs/exponential_smoother.cc
+4 -2
View File
@@ -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<float>(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();
}
src/algorithms/tracking/libs/exponential_smoother.h
+8
View File
@@ -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};
};
src/algorithms/tracking/libs/lock_detectors.cc
+29 -26
View File
@@ -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<double>(Prompt_buffer[i].real()));
Ptot += static_cast<double>(Prompt_buffer[i].imag()) * Prompt_buffer[i].imag() + static_cast<double>(Prompt_buffer[i].real()) * Prompt_buffer[i].real();
}
Psig /= static_cast<float>(length);
Psig /= static_cast<double>(length);
Psig = Psig * Psig;
Ptot /= static_cast<float>(length);
float aux = Ptot - Psig;
if (aux == 0.0)
Ptot /= static_cast<double>(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<double>(coh_integration_time_s));
return static_cast<float>(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<float>(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<double>(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<double>(Prompt_buffer[i].real()));
aux = static_cast<double>(Prompt_buffer[i].imag()) * Prompt_buffer[i].imag() + static_cast<double>(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<double>(coh_integration_time_s));
return SNR_dB_Hz;
return static_cast<float>(SNR_dB_Hz);
}