From 17f3465f3aad810662071fa5bfdd5b2dc10aa310 Mon Sep 17 00:00:00 2001 From: David Protzman Date: Sat, 11 Jun 2022 12:56:32 -0400 Subject: [PATCH] Initial commit for normalized cross correlation function This is the crazy slow version! --- .../gr-droneid/include/droneid/CMakeLists.txt | 3 +- .../include/droneid/normalized_xcorr.h | 59 ++++++++++++++ gnuradio/gr-droneid/lib/CMakeLists.txt | 4 +- gnuradio/gr-droneid/lib/normalized_xcorr.cc | 79 +++++++++++++++++++ .../gr-droneid/lib/qa_normalized_xcorr.cc | 74 +++++++++++++++++ gnuradio/gr-droneid/lib/qa_normalized_xcorr.h | 44 +++++++++++ gnuradio/gr-droneid/swig/droneid_swig.i | 2 + 7 files changed, 263 insertions(+), 2 deletions(-) create mode 100644 gnuradio/gr-droneid/include/droneid/normalized_xcorr.h create mode 100644 gnuradio/gr-droneid/lib/normalized_xcorr.cc create mode 100644 gnuradio/gr-droneid/lib/qa_normalized_xcorr.cc create mode 100644 gnuradio/gr-droneid/lib/qa_normalized_xcorr.h diff --git a/gnuradio/gr-droneid/include/droneid/CMakeLists.txt b/gnuradio/gr-droneid/include/droneid/CMakeLists.txt index cfbe114..204ecba 100644 --- a/gnuradio/gr-droneid/include/droneid/CMakeLists.txt +++ b/gnuradio/gr-droneid/include/droneid/CMakeLists.txt @@ -29,5 +29,6 @@ install(FILES demodulation.h misc_utils.h lte_decode.h - decode.h DESTINATION include/droneid + decode.h + normalized_xcorr.h DESTINATION include/droneid ) diff --git a/gnuradio/gr-droneid/include/droneid/normalized_xcorr.h b/gnuradio/gr-droneid/include/droneid/normalized_xcorr.h new file mode 100644 index 0000000..14007fe --- /dev/null +++ b/gnuradio/gr-droneid/include/droneid/normalized_xcorr.h @@ -0,0 +1,59 @@ +/* -*- c++ -*- */ +/* + * Copyright 2022 gr-droneid author. + * + * This 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, or (at your option) + * any later version. + * + * This software 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 this software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#ifndef INCLUDED_DRONEID_NORMALIZED_XCORR_H +#define INCLUDED_DRONEID_NORMALIZED_XCORR_H + +#include +#include +#include + +namespace gr { + namespace droneid { + + /*! + * \brief <+description+> + * + */ + class DRONEID_API normalized_xcorr { + public: + using sample_t = float; + using complex_t = std::complex; + using complex_vec_t = std::vector; + + explicit normalized_xcorr(const complex_vec_t & filter_taps); + + uint32_t run(const complex_t * samples, uint32_t sample_count, sample_t * output_samples); + + ~normalized_xcorr(); + protected: + complex_vec_t taps_; + double taps_var_; + uint32_t window_size_; + + complex_vec_t temp_; + private: + }; + + } // namespace droneid +} // namespace gr + +#endif /* INCLUDED_DRONEID_NORMALIZED_XCORR_H */ + diff --git a/gnuradio/gr-droneid/lib/CMakeLists.txt b/gnuradio/gr-droneid/lib/CMakeLists.txt index 032f1c9..0b38bf0 100644 --- a/gnuradio/gr-droneid/lib/CMakeLists.txt +++ b/gnuradio/gr-droneid/lib/CMakeLists.txt @@ -31,6 +31,7 @@ list(APPEND droneid_sources misc_utils.cc lte_decode.cc decode_impl.cc + normalized_xcorr.cc ) set(droneid_sources "${droneid_sources}" PARENT_SCOPE) @@ -75,6 +76,7 @@ include(GrTest) #include_directories() # List all files that contain Boost.UTF unit tests here list(APPEND test_droneid_sources + qa_normalized_xcorr.cc ) # Anything we need to link to for the unit tests go here list(APPEND GR_TEST_TARGET_DEPS gnuradio-droneid) @@ -88,4 +90,4 @@ foreach(qa_file ${test_droneid_sources}) GR_ADD_CPP_TEST("droneid_${qa_file}" ${CMAKE_CURRENT_SOURCE_DIR}/${qa_file} ) -endforeach(qa_file) +endforeach(qa_file) \ No newline at end of file diff --git a/gnuradio/gr-droneid/lib/normalized_xcorr.cc b/gnuradio/gr-droneid/lib/normalized_xcorr.cc new file mode 100644 index 0000000..ec7a0c3 --- /dev/null +++ b/gnuradio/gr-droneid/lib/normalized_xcorr.cc @@ -0,0 +1,79 @@ +/* -*- c++ -*- */ +/* + * Copyright 2022 gr-droneid author. + * + * This 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, or (at your option) + * any later version. + * + * This software 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 this software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include +#include +#include + +#include + +namespace gr { + namespace droneid { + + normalized_xcorr::normalized_xcorr(const complex_vec_t & filter_taps) { + taps_ = misc_utils::conj(filter_taps); + const auto mean = misc_utils::mean(taps_); + std::for_each(taps_.begin(), taps_.end(), [&mean](complex_t & sample){ + sample -= mean; + }); + + taps_var_ = misc_utils::var(taps_); + window_size_ = filter_taps.size(); + + temp_.resize(window_size_); + } + + normalized_xcorr::~normalized_xcorr() = default; + + uint32_t normalized_xcorr::run(const normalized_xcorr::complex_t * const samples, const uint32_t sample_count, + normalized_xcorr::sample_t * const output_samples) { + if (sample_count < window_size_) { + return 0; + } + + const auto max_correlations = sample_count - window_size_; + + 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; + } + + const auto var = misc_utils::var_no_mean(temp_); + + 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); + } + + return max_correlations; + } + + } /* namespace droneid */ +} /* namespace gr */ + diff --git a/gnuradio/gr-droneid/lib/qa_normalized_xcorr.cc b/gnuradio/gr-droneid/lib/qa_normalized_xcorr.cc new file mode 100644 index 0000000..6865db9 --- /dev/null +++ b/gnuradio/gr-droneid/lib/qa_normalized_xcorr.cc @@ -0,0 +1,74 @@ +/* -*- c++ -*- */ +/* + * Copyright 2022 gr-droneid author. + * + * This 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, or (at your option) + * any later version. + * + * This software 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 this software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#include +#include +#include "qa_normalized_xcorr.h" +#include +#include +#include +#include +#include + +#include + +namespace gr { + namespace droneid { + using sample_t = float; + using complex_t = std::complex; + using complex_vec_t = std::vector; + + BOOST_AUTO_TEST_CASE(foo) { + auto rng = gr::random(); + complex_vec_t noise_vector(1e6, {0, 0}); + for (auto & sample : noise_vector) { + sample = rng.rayleigh_complex(); + } + + const auto start_offset = 102313; // Just a random starting offset that's not on an even boundary + const auto filter_length = 1023; + + const auto filter_taps = complex_vec_t(noise_vector.begin() + start_offset, noise_vector.begin() + start_offset + filter_length); + + + normalized_xcorr xcorr(filter_taps); + + std::vector mags(noise_vector.size() - filter_length, 0); + + const auto start = std::chrono::high_resolution_clock::now(); + xcorr.run(&noise_vector[0], noise_vector.size(), &mags[0]); + const auto end = std::chrono::high_resolution_clock::now(); + const auto duration = std::chrono::duration(end - start).count(); + const auto rate = noise_vector.size() / duration; + std::cout << "Took " << duration << " seconds to run through " << noise_vector.size() << " samples\n"; + std::cout << "Average of " << rate << " samples per second\n"; + + + misc_utils::write("/tmp/mags", &mags[0], sizeof(mags[0]), mags.size()); + + const auto max_element_iter = std::max_element(mags.begin(), mags.end()); + const auto distance = std::distance(mags.begin(), max_element_iter); + std::cout << "Distance: " << distance << "\n"; + } + + + } /* namespace droneid */ +} /* namespace gr */ + diff --git a/gnuradio/gr-droneid/lib/qa_normalized_xcorr.h b/gnuradio/gr-droneid/lib/qa_normalized_xcorr.h new file mode 100644 index 0000000..e23318f --- /dev/null +++ b/gnuradio/gr-droneid/lib/qa_normalized_xcorr.h @@ -0,0 +1,44 @@ +/* -*- c++ -*- */ +/* + * Copyright 2022 gr-droneid author. + * + * This 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, or (at your option) + * any later version. + * + * This software 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 this software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#ifndef _QA_NORMALIZED_XCORR_H_ +#define _QA_NORMALIZED_XCORR_H_ + +#include +#include + +namespace gr { + namespace droneid { + + class qa_normalized_xcorr : public CppUnit::TestCase { + public: + CPPUNIT_TEST_SUITE(qa_normalized_xcorr); + CPPUNIT_TEST(t1); + CPPUNIT_TEST_SUITE_END(); + + private: + void t1(); + }; + + } /* namespace droneid */ +} /* namespace gr */ + +#endif /* _QA_NORMALIZED_XCORR_H_ */ + diff --git a/gnuradio/gr-droneid/swig/droneid_swig.i b/gnuradio/gr-droneid/swig/droneid_swig.i index b6d3226..549601c 100644 --- a/gnuradio/gr-droneid/swig/droneid_swig.i +++ b/gnuradio/gr-droneid/swig/droneid_swig.i @@ -15,6 +15,7 @@ #include "droneid/misc_utils.h" #include "droneid/lte_decode.h" #include "droneid/decode.h" +#include "droneid/normalized_xcorr.h" //#include "droneid/utils.h" %} @@ -35,3 +36,4 @@ GR_SWIG_BLOCK_MAGIC2(droneid, demodulation); %include "droneid/lte_decode.h" %include "droneid/decode.h" GR_SWIG_BLOCK_MAGIC2(droneid, decode); +%include "droneid/normalized_xcorr.h"