Add interpolate in delay and Doppler

This commit is contained in:
30hours 2023-12-03 04:20:49 +00:00
parent 0c308dfec6
commit c5d99f8058
6 changed files with 138 additions and 21 deletions

View file

@ -23,6 +23,7 @@ add_executable(blah2
${PROJECT_SOURCE_DIR}/process/clutter/WienerHopf.cpp ${PROJECT_SOURCE_DIR}/process/clutter/WienerHopf.cpp
${PROJECT_SOURCE_DIR}/process/detection/CfarDetector1D.cpp ${PROJECT_SOURCE_DIR}/process/detection/CfarDetector1D.cpp
${PROJECT_SOURCE_DIR}/process/detection/Centroid.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/IqData.cpp
${PROJECT_SOURCE_DIR}/data/Map.cpp ${PROJECT_SOURCE_DIR}/data/Map.cpp
${PROJECT_SOURCE_DIR}/data/Detection.cpp ${PROJECT_SOURCE_DIR}/data/Detection.cpp

View file

@ -14,6 +14,7 @@
#include <Map.h> #include <Map.h>
#include <Detection.h> #include <Detection.h>
#include <Centroid.h> #include <Centroid.h>
#include <Interpolate.h>
#include <Timing.h> #include <Timing.h>
#include <sys/types.h> #include <sys/types.h>
#include <getopt.h> #include <getopt.h>
@ -88,6 +89,7 @@ int main(int argc, char **argv)
std::string mapJson, detectionJson; std::string mapJson, detectionJson;
Detection *detection; Detection *detection;
Detection *detection1; Detection *detection1;
Detection *detection2;
// setup fftw multithread // setup fftw multithread
if (fftw_init_threads() == 0) if (fftw_init_threads() == 0)
@ -156,6 +158,7 @@ int main(int argc, char **argv)
tree["process"]["detection"]["minDelay"] >> minDelay; tree["process"]["detection"]["minDelay"] >> minDelay;
tree["process"]["detection"]["minDoppler"] >> minDoppler; tree["process"]["detection"]["minDoppler"] >> minDoppler;
CfarDetector1D *cfarDetector1D = new CfarDetector1D(pfa, nGuard, nTrain, minDelay, minDoppler); CfarDetector1D *cfarDetector1D = new CfarDetector1D(pfa, nGuard, nTrain, minDelay, minDoppler);
Interpolate *interpolate = new Interpolate(true, true);
// setup process centroid // setup process centroid
uint16_t nCentroid; uint16_t nCentroid;
@ -229,7 +232,9 @@ int main(int argc, char **argv)
// detection process // detection process
detection1 = cfarDetector1D->process(map); detection1 = cfarDetector1D->process(map);
detection = centroid->process(detection1); detection2 = centroid->process(detection1);
detection = interpolate->process(detection2, map);
uint64_t t4 = current_time_us(); uint64_t t4 = current_time_us();
double delta_t4 = (double)(t4-t3) / 1000; double delta_t4 = (double)(t4-t3) / 1000;
timing_name.push_back("detector"); timing_name.push_back("detector");
@ -258,6 +263,7 @@ int main(int argc, char **argv)
} }
delete detection; delete detection;
delete detection1; delete detection1;
delete detection2;
// output radar data timer // output radar data timer
uint64_t t5 = current_time_us(); uint64_t t5 = current_time_us();

View file

@ -98,6 +98,20 @@ void Map<T>::print()
} }
} }
template <class T>
uint32_t Map<T>::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 <class T> template <class T>
std::string Map<T>::to_json() std::string Map<T>::to_json()
{ {

View file

@ -89,6 +89,11 @@ public:
/// @return Void. /// @return Void.
void print(); 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. /// @brief Generate JSON of the map and metadata.
/// @return JSON string. /// @return JSON string.
std::string to_json(); std::string to_json();

View file

@ -0,0 +1,93 @@
#include "Interpolate.h"
#include <iostream>
#include <vector>
#include <cmath>
#include <stdint.h>
#include <algorithm>
// constructor
Interpolate::Interpolate(bool _doDelay, bool _doDoppler)
{
// input
doDelay = _doDelay;
doDoppler = _doDoppler;
}
Interpolate::~Interpolate()
{
}
Detection *Interpolate::process(Detection *x, Map<std::complex<double>> *y)
{
// store detections temporarily
std::vector<double> 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<double> delay2, doppler2, snr2;
std::deque<int> indexDelay = y->delay;
std::deque<double> 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;
}

View file

@ -1,39 +1,37 @@
/// @file Interpolate.h /// @file Interpolate.h
/// @class Interpolate /// @class Interpolate
/// @brief A class to remove duplicate target detections. /// @brief A class to interpolate detection data using a quadratic curve.
/// @details If detection SNR is larger than neighbours, then remove. /// @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 /// @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 #ifndef INTERPOLATE_H
#define CENTROID_H #define INTERPOLATE_H
#include <Map.h> #include <Map.h>
#include <Detection.h> #include <Detection.h>
#include <stdint.h>
class Centroid class Interpolate
{ {
private: private:
/// @brief Number of delay bins to check. /// @brief True if interpolating over delay.
int8_t nDelay; bool doDelay;
/// @brief Number of Doppler bins to check. /// @brief True if interpolating over Doppler.
int8_t nDoppler; bool doDoppler;
/// @brief Doppler resolution to convert Hz to bins (Hz).
double resolutionDoppler;
/// @brief Pointer to detection data to store result. /// @brief Pointer to detection data to store result.
Detection *detection; Detection *detection;
public: public:
/// @brief Constructor. /// @brief Constructor.
/// @param nDelay Number of delay bins to check. /// @param doDelay True if interpolating over delay.
/// @param nDoppler Number of Doppler bins to check. /// @param doDoppler True if interpolating over Doppler.
/// @param resolutionDoppler Doppler resolution to convert Hz to bins (Hz).
/// @return The object. /// @return The object.
Interpolate(int8_t nDelay, int8_t nDoppler, double resolutionDoppler); Interpolate(bool doDelay, bool doDoppler);
/// @brief Destructor. /// @brief Destructor.
/// @return Void. /// @return Void.
@ -41,8 +39,8 @@ public:
/// @brief Implement the 1D CFAR detector. /// @brief Implement the 1D CFAR detector.
/// @param x Detections from the 1D CFAR detector. /// @param x Detections from the 1D CFAR detector.
/// @return Centroided detections. /// @return Interpolated detections.
Detection *process(Detection *x); Detection *process(Detection *x, Map<std::complex<double>> *y);
}; };
#endif #endif