From c89d6e6004870fc0cc010521c7634a71408addce Mon Sep 17 00:00:00 2001 From: David Protzman Date: Sun, 19 Jun 2022 13:42:36 -0400 Subject: [PATCH] Normalization is working properly, but it's crazy slow --- .../include/droneid/normalized_xcorr.h | 1 + gnuradio/gr-droneid/lib/normalized_xcorr.cc | 30 +++++++++++-------- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/gnuradio/gr-droneid/include/droneid/normalized_xcorr.h b/gnuradio/gr-droneid/include/droneid/normalized_xcorr.h index 14007fe..4443366 100644 --- a/gnuradio/gr-droneid/include/droneid/normalized_xcorr.h +++ b/gnuradio/gr-droneid/include/droneid/normalized_xcorr.h @@ -49,6 +49,7 @@ namespace gr { uint32_t window_size_; complex_vec_t temp_; + complex_vec_t scores_; private: }; diff --git a/gnuradio/gr-droneid/lib/normalized_xcorr.cc b/gnuradio/gr-droneid/lib/normalized_xcorr.cc index ec7a0c3..597d0fa 100644 --- a/gnuradio/gr-droneid/lib/normalized_xcorr.cc +++ b/gnuradio/gr-droneid/lib/normalized_xcorr.cc @@ -53,24 +53,30 @@ namespace gr { } const auto max_correlations = sample_count - window_size_; + if (scores_.size() < max_correlations) { + scores_.resize(max_correlations); + } - complex_t dot_prod; - for (uint32_t idx = 0; idx < max_correlations; idx++) { - const auto mean = misc_utils::mean(samples + idx, window_size_); - for (uint32_t sample_idx = 0; sample_idx < window_size_; sample_idx++) { - temp_[sample_idx] = samples[idx + sample_idx] - mean; + for (auto offset = decltype(max_correlations){0}; offset < max_correlations; offset++) { + std::copy(samples + offset, samples + offset + window_size_, temp_.begin()); + + const auto mean = misc_utils::mean(temp_); + std::for_each(temp_.begin(), temp_.end(), [&mean](complex_t & sample){ + sample -= mean; + }); + + complex_t sum = {0, 0}; + for (auto idx = decltype(window_size_){0}; idx < window_size_; idx++) { + sum += temp_[idx] * taps_[idx]; } - const auto var = misc_utils::var_no_mean(temp_); + sum /= static_cast(window_size_); - volk_32fc_x2_dot_prod_32fc(&dot_prod, &temp_[0], &taps_[0], window_size_); - dot_prod /= static_cast(window_size_); - - const auto score = dot_prod / static_cast(std::sqrt(var * taps_var_)); - output_samples[idx] = static_cast(std::pow(score.real(), 2) + std::pow(score.imag(), 2)); - //output_samples[idx] = std::pow(std::abs(dot_prod), 1); + scores_[offset] = sum / static_cast(std::sqrt(taps_var_ * misc_utils::var_no_mean(temp_))); } + volk_32fc_magnitude_squared_32f(output_samples, &scores_[0], scores_.size()); + return max_correlations; }