From c5d99f8058fd1732db1b3254fa273129ee8ac5c4 Mon Sep 17 00:00:00 2001 From: 30hours Date: Sun, 3 Dec 2023 04:20:49 +0000 Subject: [PATCH] Add interpolate in delay and Doppler --- CMakeLists.txt | 1 + src/blah2.cpp | 8 ++- src/data/Map.cpp | 14 ++++ src/data/Map.h | 5 ++ src/process/detection/Interpolate.cpp | 93 +++++++++++++++++++++++++++ src/process/detection/Interpolate.h | 38 ++++++----- 6 files changed, 138 insertions(+), 21 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cf62050..c51e699 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,6 +23,7 @@ add_executable(blah2 ${PROJECT_SOURCE_DIR}/process/clutter/WienerHopf.cpp ${PROJECT_SOURCE_DIR}/process/detection/CfarDetector1D.cpp ${PROJECT_SOURCE_DIR}/process/detection/Centroid.cpp + ${PROJECT_SOURCE_DIR}/process/detection/Interpolate.cpp ${PROJECT_SOURCE_DIR}/data/IqData.cpp ${PROJECT_SOURCE_DIR}/data/Map.cpp ${PROJECT_SOURCE_DIR}/data/Detection.cpp diff --git a/src/blah2.cpp b/src/blah2.cpp index 9ccc6b8..ab07fc6 100644 --- a/src/blah2.cpp +++ b/src/blah2.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -88,6 +89,7 @@ int main(int argc, char **argv) std::string mapJson, detectionJson; Detection *detection; Detection *detection1; + Detection *detection2; // setup fftw multithread if (fftw_init_threads() == 0) @@ -156,6 +158,7 @@ int main(int argc, char **argv) tree["process"]["detection"]["minDelay"] >> minDelay; tree["process"]["detection"]["minDoppler"] >> minDoppler; CfarDetector1D *cfarDetector1D = new CfarDetector1D(pfa, nGuard, nTrain, minDelay, minDoppler); + Interpolate *interpolate = new Interpolate(true, true); // setup process centroid uint16_t nCentroid; @@ -229,7 +232,9 @@ int main(int argc, char **argv) // detection process detection1 = cfarDetector1D->process(map); - detection = centroid->process(detection1); + detection2 = centroid->process(detection1); + detection = interpolate->process(detection2, map); + uint64_t t4 = current_time_us(); double delta_t4 = (double)(t4-t3) / 1000; timing_name.push_back("detector"); @@ -258,6 +263,7 @@ int main(int argc, char **argv) } delete detection; delete detection1; + delete detection2; // output radar data timer uint64_t t5 = current_time_us(); diff --git a/src/data/Map.cpp b/src/data/Map.cpp index d0d99af..e61f454 100644 --- a/src/data/Map.cpp +++ b/src/data/Map.cpp @@ -98,6 +98,20 @@ void Map::print() } } +template +uint32_t Map::doppler_hz_to_bin(double dopplerHz) +{ + for (size_t i = 0; i < doppler.size(); i++) + { + if (dopplerHz == doppler[i]) + { + return (int) i; + } + } + std::cout << "HUGE FUCKUP GRR" << std::endl; + return 0; +} + template std::string Map::to_json() { diff --git a/src/data/Map.h b/src/data/Map.h index a094cad..7f9f665 100644 --- a/src/data/Map.h +++ b/src/data/Map.h @@ -89,6 +89,11 @@ public: /// @return Void. void print(); + /// @brief Convert a Doppler value from Hz to bins. + /// @param dopplerHz Doppler value (Hz). + /// @return dopplerBin Doppler value (bins). + uint32_t doppler_hz_to_bin(double dopplerHz); + /// @brief Generate JSON of the map and metadata. /// @return JSON string. std::string to_json(); diff --git a/src/process/detection/Interpolate.cpp b/src/process/detection/Interpolate.cpp index e69de29..b807089 100644 --- a/src/process/detection/Interpolate.cpp +++ b/src/process/detection/Interpolate.cpp @@ -0,0 +1,93 @@ +#include "Interpolate.h" +#include +#include +#include +#include +#include + +// constructor +Interpolate::Interpolate(bool _doDelay, bool _doDoppler) +{ + // input + doDelay = _doDelay; + doDoppler = _doDoppler; +} + +Interpolate::~Interpolate() +{ +} + +Detection *Interpolate::process(Detection *x, Map> *y) +{ + // store detections temporarily + std::vector delay, doppler, snr; + delay = x->get_delay(); + doppler = x->get_doppler(); + snr = x->get_snr(); + + // interpolate data + double intDelay, intDoppler, intSnrDelay, intSnrDoppler, intSnr[3]; + std::vector delay2, doppler2, snr2; + std::deque indexDelay = y->delay; + std::deque indexDoppler = y->doppler; + + // loop over every detection + for (size_t i = 0; i < snr.size(); i++) + { + // initialise interpolated values for bool flags + intDelay = delay[i]; + intDoppler = doppler[i]; + intSnrDelay = snr[i]; + intSnrDoppler = snr[i]; + // interpolate in delay + if (doDelay) + { + // check not on boundary + if (delay[i] == indexDelay[0] || delay[i] == indexDelay.back()) + { + continue; + } + intSnr[0] = (double)10*std::log10(std::abs(y->data[y->doppler_hz_to_bin(doppler[i])][delay[i]-1-indexDelay[0]]))-y->noisePower; + intSnr[1] = (double)10*std::log10(std::abs(y->data[y->doppler_hz_to_bin(doppler[i])][delay[i]-indexDelay[0]]))-y->noisePower; + intSnr[2] = (double)10*std::log10(std::abs(y->data[y->doppler_hz_to_bin(doppler[i])][delay[i]+1-indexDelay[0]]))-y->noisePower; + // check detection has peak SNR of neighbours + if (intSnr[1] < intSnr[0] || intSnr[1] < intSnr[2]) + { + std::cout << "Detection dropped (SNR of peak lower)" << std::endl; + continue; + } + intDelay = (intSnr[0]-intSnr[2])/(2*(intSnr[0]-(2*intSnr[1])+intSnr[2])); + intSnrDelay = intSnr[1] - (((intSnr[0]-intSnr[2])*intDelay)/4); + intDelay = delay[i] + intDelay; + } + // interpolate in Doppler + if (doDoppler) + { + // check not on boundary + if (doppler[i] == indexDoppler[0] || doppler[i] == indexDoppler.back()) + { + continue; + } + intSnr[0] = (double)10*std::log10(std::abs(y->data[y->doppler_hz_to_bin(doppler[i])-1][delay[i]-indexDelay[0]]))-y->noisePower; + intSnr[1] = (double)10*std::log10(std::abs(y->data[y->doppler_hz_to_bin(doppler[i])][delay[i]-indexDelay[0]]))-y->noisePower; + intSnr[2] = (double)10*std::log10(std::abs(y->data[y->doppler_hz_to_bin(doppler[i])+1][delay[i]-indexDelay[0]]))-y->noisePower; + // check detection has peak SNR of neighbours + if (intSnr[1] < intSnr[0] || intSnr[1] < intSnr[2]) + { + continue; + } + intDoppler = (intSnr[0]-intSnr[2])/(2*(intSnr[0]-(2*intSnr[1])+intSnr[2])); + intSnrDelay = intSnr[1] - (((intSnr[0]-intSnr[2])*intDoppler)/4); + intDoppler = doppler[i] + ((indexDoppler[1]-indexDoppler[0])*intDoppler); + } + // store interpolated detections + delay2.push_back(intDelay); + doppler2.push_back(intDoppler); + snr2.push_back(std::max(std::max(intSnrDelay, intSnrDoppler), snr[i])); + } + + // create detection + Detection *detection = new Detection(delay2, doppler2, snr2); + + return detection; +} diff --git a/src/process/detection/Interpolate.h b/src/process/detection/Interpolate.h index 9f01577..11afce9 100644 --- a/src/process/detection/Interpolate.h +++ b/src/process/detection/Interpolate.h @@ -1,39 +1,37 @@ /// @file Interpolate.h /// @class Interpolate -/// @brief A class to remove duplicate target detections. -/// @details If detection SNR is larger than neighbours, then remove. +/// @brief A class to interpolate detection data using a quadratic curve. +/// @details Interpolate in delay and Doppler. If 2 points either side have a higher SNR, then remove detection. +/// References: +/// - https://ccrma.stanford.edu/~jos/sasp/Quadratic_Interpolation_Spectral_Peaks.html +/// - Fundamentals of Signal Processing (2nd), Richards, Section 5.3.6 /// @author 30hours -/// @todo Still a bug where sometimes 2 consecutive range detections get through. +/// @todo Should I remove the detection pointer? Also on Centroid. -#ifndef CENTROID_H -#define CENTROID_H +#ifndef INTERPOLATE_H +#define INTERPOLATE_H #include #include -#include -class Centroid +class Interpolate { private: - /// @brief Number of delay bins to check. - int8_t nDelay; + /// @brief True if interpolating over delay. + bool doDelay; - /// @brief Number of Doppler bins to check. - int8_t nDoppler; - - /// @brief Doppler resolution to convert Hz to bins (Hz). - double resolutionDoppler; + /// @brief True if interpolating over Doppler. + bool doDoppler; /// @brief Pointer to detection data to store result. Detection *detection; public: /// @brief Constructor. - /// @param nDelay Number of delay bins to check. - /// @param nDoppler Number of Doppler bins to check. - /// @param resolutionDoppler Doppler resolution to convert Hz to bins (Hz). + /// @param doDelay True if interpolating over delay. + /// @param doDoppler True if interpolating over Doppler. /// @return The object. - Interpolate(int8_t nDelay, int8_t nDoppler, double resolutionDoppler); + Interpolate(bool doDelay, bool doDoppler); /// @brief Destructor. /// @return Void. @@ -41,8 +39,8 @@ public: /// @brief Implement the 1D CFAR detector. /// @param x Detections from the 1D CFAR detector. - /// @return Centroided detections. - Detection *process(Detection *x); + /// @return Interpolated detections. + Detection *process(Detection *x, Map> *y); }; #endif