Initial commit for normalized cross correlation function

This is the crazy slow version!

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
 )

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 <droneid/api.h>
+#include <vector>
+#include <complex>
+
+namespace gr {
+    namespace droneid {
+
+        /*!
+         * \brief <+description+>
+         *
+         */
+        class DRONEID_API normalized_xcorr {
+        public:
+            using sample_t = float;
+            using complex_t = std::complex<sample_t>;
+            using complex_vec_t = std::vector<complex_t>;
+
+            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 */
+

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)

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 <gnuradio/io_signature.h>
+#include <droneid/normalized_xcorr.h>
+#include <droneid/misc_utils.h>
+
+#include <volk/volk.h>
+
+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<sample_t>(window_size_);
+
+                const auto score = dot_prod / static_cast<sample_t>(std::sqrt(var * taps_var_));
+                output_samples[idx] = static_cast<sample_t>(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 */
+

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 <gnuradio/attributes.h>
+#include <cppunit/TestAssert.h>
+#include "qa_normalized_xcorr.h"
+#include <droneid/normalized_xcorr.h>
+#include <iostream>
+#include <boost/test/unit_test.hpp>
+#include <gnuradio/random.h>
+#include <chrono>
+
+#include <droneid/misc_utils.h>
+
+namespace gr {
+  namespace droneid {
+      using sample_t = float;
+      using complex_t = std::complex<sample_t>;
+      using complex_vec_t = std::vector<complex_t>;
+
+    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<sample_t> 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<float>(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 */
+

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 <cppunit/extensions/HelperMacros.h>
+#include <cppunit/TestCase.h>
+
+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_ */
+

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"