mirror of
https://github.com/30hours/blah2.git
synced 2024-11-08 12:25:42 +00:00
Add interpolate in delay and Doppler
This commit is contained in:
parent
0c308dfec6
commit
c5d99f8058
6 changed files with 138 additions and 21 deletions
|
@ -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
|
||||||
|
|
|
@ -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();
|
||||||
|
|
|
@ -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()
|
||||||
{
|
{
|
||||||
|
|
|
@ -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();
|
||||||
|
|
|
@ -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;
|
||||||
|
}
|
|
@ -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
|
||||||
|
|
Loading…
Reference in a new issue