mirror of
https://github.com/30hours/blah2.git
synced 2024-11-18 12:33:58 +00:00
Work in progress on TestTracker updates
This commit is contained in:
parent
addb8c49dd
commit
69eef1a5f2
5 changed files with 90 additions and 25 deletions
|
@ -19,6 +19,9 @@ find_package(Catch2 CONFIG REQUIRED)
|
||||||
SET (PROJECT_ROOT "${PROJECT_SOURCE_DIR}")
|
SET (PROJECT_ROOT "${PROJECT_SOURCE_DIR}")
|
||||||
SET (CMAKE_RUNTIME_OUTPUT_DIRECTORY "${PROJECT_ROOT}/bin")
|
SET (CMAKE_RUNTIME_OUTPUT_DIRECTORY "${PROJECT_ROOT}/bin")
|
||||||
SET (PROJECT_BINARY_TEST_DIR "${PROJECT_ROOT}/bin/test")
|
SET (PROJECT_BINARY_TEST_DIR "${PROJECT_ROOT}/bin/test")
|
||||||
|
SET (PROJECT_BINARY_TEST_UNIT_DIR "${PROJECT_BINARY_TEST_DIR}/unit")
|
||||||
|
SET (PROJECT_BINARY_TEST_UNIT_DIR "${PROJECT_BINARY_TEST_DIR}/functional")
|
||||||
|
SET (PROJECT_BINARY_TEST_UNIT_DIR "${PROJECT_BINARY_TEST_DIR}/comparison")
|
||||||
|
|
||||||
MESSAGE ("Binary path: ${PROJECT_BINARY_DIR}")
|
MESSAGE ("Binary path: ${PROJECT_BINARY_DIR}")
|
||||||
MESSAGE ("Binary test path: ${PROJECT_BINARY_TEST_DIR}")
|
MESSAGE ("Binary test path: ${PROJECT_BINARY_TEST_DIR}")
|
||||||
|
|
|
@ -186,14 +186,15 @@ int main(int argc, char **argv)
|
||||||
|
|
||||||
// setup process tracker
|
// setup process tracker
|
||||||
uint8_t m, n, nDelete;
|
uint8_t m, n, nDelete;
|
||||||
double maxAcc, rangeRes;
|
double maxAcc, rangeRes, lambda;
|
||||||
std::string smooth;
|
std::string smooth;
|
||||||
tree["process"]["tracker"]["initiate"]["M"] >> m;
|
tree["process"]["tracker"]["initiate"]["M"] >> m;
|
||||||
tree["process"]["tracker"]["initiate"]["N"] >> n;
|
tree["process"]["tracker"]["initiate"]["N"] >> n;
|
||||||
tree["process"]["tracker"]["delete"] >> nDelete;
|
tree["process"]["tracker"]["delete"] >> nDelete;
|
||||||
tree["process"]["tracker"]["initiate"]["maxAcc"] >> maxAcc;
|
tree["process"]["tracker"]["initiate"]["maxAcc"] >> maxAcc;
|
||||||
rangeRes = 299792458.0/fs;
|
rangeRes = 299792458.0/fs;
|
||||||
Tracker *tracker = new Tracker(m, n, nDelete, tCpi, maxAcc, rangeRes);
|
lambda = 299792458.0/fc;
|
||||||
|
Tracker *tracker = new Tracker(m, n, nDelete, ambiguity->cpi_length_seconds(), maxAcc, rangeRes, lambda);
|
||||||
|
|
||||||
// setup process spectrum analyser
|
// setup process spectrum analyser
|
||||||
double spectrumBandwidth = 2000;
|
double spectrumBandwidth = 2000;
|
||||||
|
|
|
@ -2,7 +2,8 @@
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
|
||||||
// constructor
|
// constructor
|
||||||
Tracker::Tracker(uint32_t _m, uint32_t _n, uint32_t _nDelete, double _cpi, double _maxAccInit, double _rangeRes)
|
Tracker::Tracker(uint32_t _m, uint32_t _n, uint32_t _nDelete,
|
||||||
|
double _cpi, double _maxAccInit, double _rangeRes, double _lambda)
|
||||||
{
|
{
|
||||||
m = _m;
|
m = _m;
|
||||||
n = _n;
|
n = _n;
|
||||||
|
@ -11,6 +12,7 @@ Tracker::Tracker(uint32_t _m, uint32_t _n, uint32_t _nDelete, double _cpi, doubl
|
||||||
maxAccInit = _maxAccInit;
|
maxAccInit = _maxAccInit;
|
||||||
timestamp = 0;
|
timestamp = 0;
|
||||||
rangeRes = _rangeRes;
|
rangeRes = _rangeRes;
|
||||||
|
lambda = _lambda;
|
||||||
|
|
||||||
double resolutionAcc = 1/(cpi*cpi);
|
double resolutionAcc = 1/(cpi*cpi);
|
||||||
uint16_t nAcc = (int)maxAccInit/resolutionAcc;
|
uint16_t nAcc = (int)maxAccInit/resolutionAcc;
|
||||||
|
@ -67,12 +69,8 @@ void Tracker::update(Detection *detection, uint64_t current)
|
||||||
{
|
{
|
||||||
// predict next position
|
// predict next position
|
||||||
Detection detectionCurrent = track.get_current(i);
|
Detection detectionCurrent = track.get_current(i);
|
||||||
double delayTrack = detectionCurrent.get_delay().front();
|
|
||||||
double dopplerTrack = detectionCurrent.get_doppler().front();
|
|
||||||
acc = track.get_acceleration(i);
|
acc = track.get_acceleration(i);
|
||||||
delayPredict = delayTrack+((dopplerTrack*T)+(0.5*acc*T*T))/rangeRes;
|
Detection prediction = predict(detectionCurrent, acc, T);
|
||||||
dopplerPredict = dopplerTrack+(acc*T);
|
|
||||||
Detection prediction(delayPredict, dopplerPredict, 0);
|
|
||||||
|
|
||||||
// loop over detections to associate
|
// loop over detections to associate
|
||||||
for (size_t j = 0; j < detection->get_nDetections(); j++)
|
for (size_t j = 0; j < detection->get_nDetections(); j++)
|
||||||
|
@ -85,7 +83,7 @@ void Tracker::update(Detection *detection, uint64_t current)
|
||||||
{
|
{
|
||||||
Detection associated(delay[j], doppler[j], snr[j]);
|
Detection associated(delay[j], doppler[j], snr[j]);
|
||||||
track.set_current(i, associated);
|
track.set_current(i, associated);
|
||||||
track.set_acceleration(i, (doppler[j]-dopplerTrack)/T);
|
track.set_acceleration(i, (doppler[j]-detectionCurrent.get_doppler().front())/T);
|
||||||
track.set_nInactive(i, 0);
|
track.set_nInactive(i, 0);
|
||||||
doNotInitiate[j] = true;
|
doNotInitiate[j] = true;
|
||||||
state = "ASSOCIATED";
|
state = "ASSOCIATED";
|
||||||
|
@ -123,6 +121,17 @@ void Tracker::update(Detection *detection, uint64_t current)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Detection Tracker::predict(Detection current, double acc, double T)
|
||||||
|
{
|
||||||
|
double delayTrack = current.get_delay().front();
|
||||||
|
double dopplerTrack = current.get_doppler().front();
|
||||||
|
double delayPredict = delayTrack+((dopplerTrack*T*lambda)+
|
||||||
|
(0.5*acc*T*T))/rangeRes;
|
||||||
|
double dopplerPredict = dopplerTrack+(acc*T);
|
||||||
|
Detection prediction(delayPredict, dopplerPredict, 0);
|
||||||
|
return prediction;
|
||||||
|
}
|
||||||
|
|
||||||
void Tracker::initiate(Detection *detection)
|
void Tracker::initiate(Detection *detection)
|
||||||
{
|
{
|
||||||
std::vector<double> delay = detection->get_delay();
|
std::vector<double> delay = detection->get_delay();
|
||||||
|
|
|
@ -6,6 +6,7 @@
|
||||||
/// @author 30hours
|
/// @author 30hours
|
||||||
/// @todo Add smoothing capability.
|
/// @todo Add smoothing capability.
|
||||||
/// @todo Fix units up.
|
/// @todo Fix units up.
|
||||||
|
/// @todo I don't think I callback the true CPI time from ambiguity.
|
||||||
|
|
||||||
#ifndef TRACKER_H
|
#ifndef TRACKER_H
|
||||||
#define TRACKER_H
|
#define TRACKER_H
|
||||||
|
@ -35,6 +36,9 @@ private:
|
||||||
/// @brief Range resolution for kinematics equations (m).
|
/// @brief Range resolution for kinematics equations (m).
|
||||||
double rangeRes;
|
double rangeRes;
|
||||||
|
|
||||||
|
/// @brief Wavelength for kinematics equations (m).
|
||||||
|
double lambda;
|
||||||
|
|
||||||
/// @brief Acceleration values to initiate track (Hz/s).
|
/// @brief Acceleration values to initiate track (Hz/s).
|
||||||
std::vector<double> accInit;
|
std::vector<double> accInit;
|
||||||
|
|
||||||
|
@ -49,11 +53,16 @@ private:
|
||||||
|
|
||||||
public:
|
public:
|
||||||
/// @brief Constructor.
|
/// @brief Constructor.
|
||||||
/// @param delayMin Minimum clutter filter delay (bins).
|
/// @param m Track initiation constant for M of N detections.
|
||||||
/// @param delayMax Maximum clutter filter delay (bins).
|
/// @param n Track initiation constant for M of N detections.
|
||||||
/// @param nSamples Number of samples per CPI.
|
/// @param nDelete Number of missed predictions to delete a tentative track.
|
||||||
|
/// @param cpi True CPI time for acceleration resolution(s).
|
||||||
|
/// @param maxAccInit Maximum acceleration to initiate track (Hz/s).
|
||||||
|
/// @param rangeRes Range resolution for kinematics equations (m).
|
||||||
|
/// @param lambda Wavelength for kinematics equations (m).
|
||||||
/// @return The object.
|
/// @return The object.
|
||||||
Tracker(uint32_t m, uint32_t n, uint32_t nDelete, double cpi, double maxAccInit, double rangeRes);
|
Tracker(uint32_t m, uint32_t n, uint32_t nDelete, double cpi,
|
||||||
|
double maxAccInit, double rangeRes, double lambda);
|
||||||
|
|
||||||
/// @brief Destructor.
|
/// @brief Destructor.
|
||||||
/// @return Void.
|
/// @return Void.
|
||||||
|
@ -71,6 +80,13 @@ public:
|
||||||
/// @return Void.
|
/// @return Void.
|
||||||
void update(Detection *detection, uint64_t timestamp);
|
void update(Detection *detection, uint64_t timestamp);
|
||||||
|
|
||||||
|
/// @brief Predict next bistatic position using kinematics equations.
|
||||||
|
/// @param current Current position of track.
|
||||||
|
/// @param acc Acceleration hypothesis of track.
|
||||||
|
/// @param T Time elapsed from previous CPI.
|
||||||
|
/// @return Predicted position of track.
|
||||||
|
Detection predict(Detection current, double acc, double T);
|
||||||
|
|
||||||
/// @brief Initiate new tentative tracks from detections.
|
/// @brief Initiate new tentative tracks from detections.
|
||||||
/// @param detection Detection data for last CPI.
|
/// @param detection Detection data for last CPI.
|
||||||
/// @return Void.
|
/// @return Void.
|
||||||
|
|
|
@ -3,6 +3,7 @@
|
||||||
/// @author 30hours
|
/// @author 30hours
|
||||||
|
|
||||||
#include <catch2/catch_test_macros.hpp>
|
#include <catch2/catch_test_macros.hpp>
|
||||||
|
#include <catch2/matchers/catch_matchers_floating_point.hpp>
|
||||||
|
|
||||||
#include "data/Detection.h"
|
#include "data/Detection.h"
|
||||||
#include "data/Track.h"
|
#include "data/Track.h"
|
||||||
|
@ -17,13 +18,17 @@
|
||||||
/// @details Check constructor parameters created correctly.
|
/// @details Check constructor parameters created correctly.
|
||||||
TEST_CASE("Constructor", "[constructor]")
|
TEST_CASE("Constructor", "[constructor]")
|
||||||
{
|
{
|
||||||
uint32_t m = 3;
|
uint32_t m = 3;
|
||||||
uint32_t n = 5;
|
uint32_t n = 5;
|
||||||
uint32_t nDelete = 5;
|
uint32_t nDelete = 5;
|
||||||
double cpi = 0.5;
|
double cpi = 1;
|
||||||
double maxAccInit = 10;
|
double maxAccInit = 10;
|
||||||
double rangeRes = 100;
|
double fs = 2000000;
|
||||||
Tracker tracker = Tracker(m, n, nDelete, cpi, maxAccInit, rangeRes);
|
double rangeRes = 299792458.0/fs;
|
||||||
|
double fc = 204640000;
|
||||||
|
double lambda = 299792458.0/fc;
|
||||||
|
Tracker tracker = Tracker(m, n, nDelete,
|
||||||
|
cpi, maxAccInit, rangeRes, lambda);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// @brief Test process for an ACTIVE track.
|
/// @brief Test process for an ACTIVE track.
|
||||||
|
@ -32,16 +37,47 @@ TEST_CASE("Process ACTIVE track constant acc", "[process]")
|
||||||
uint32_t m = 3;
|
uint32_t m = 3;
|
||||||
uint32_t n = 5;
|
uint32_t n = 5;
|
||||||
uint32_t nDelete = 5;
|
uint32_t nDelete = 5;
|
||||||
double cpi = 0.5;
|
double cpi = 1;
|
||||||
double maxAccInit = 10;
|
double maxAccInit = 10;
|
||||||
double rangeRes = 100;
|
double fs = 2000000;
|
||||||
|
double rangeRes = 299792458.0/fs;
|
||||||
|
double fc = 204640000;
|
||||||
|
double lambda = 299792458.0/fc;
|
||||||
Tracker tracker = Tracker(m, n, nDelete,
|
Tracker tracker = Tracker(m, n, nDelete,
|
||||||
cpi, maxAccInit, rangeRes);
|
cpi, maxAccInit, rangeRes, lambda);
|
||||||
|
|
||||||
|
|
||||||
// create detections with constant acc 5 Hz/s
|
// create detections with constant acc 5 Hz/s
|
||||||
|
std::vector<uint64_t> timestamp = {0,1,2,3,4,5,6,7,8,9,10};
|
||||||
std::vector<double> delay = {10};
|
std::vector<double> delay = {10};
|
||||||
std::vector<double> doppler = {-20};
|
std::vector<double> doppler = {-20,-15,-10,-5,0,5,10,15,20,25};
|
||||||
|
|
||||||
std::string state = "ACTIVE";
|
std::string state = "ACTIVE";
|
||||||
|
}
|
||||||
|
|
||||||
|
/// @brief Test predict for kinematics equations.
|
||||||
|
TEST_CASE("Test predict", "[predict]")
|
||||||
|
{
|
||||||
|
uint32_t m = 3;
|
||||||
|
uint32_t n = 5;
|
||||||
|
uint32_t nDelete = 5;
|
||||||
|
double cpi = 1;
|
||||||
|
double maxAccInit = 10;
|
||||||
|
double fs = 2000000;
|
||||||
|
double rangeRes = 299792458.0/fs;
|
||||||
|
double fc = 204640000;
|
||||||
|
double lambda = 299792458.0/fc;
|
||||||
|
Tracker tracker = Tracker(m, n, nDelete,
|
||||||
|
cpi, maxAccInit, rangeRes, lambda);
|
||||||
|
|
||||||
|
Detection input = Detection(10, -20, 0);
|
||||||
|
double acc = 5;
|
||||||
|
double T = 1;
|
||||||
|
Detection prediction = tracker.predict(input, acc, T);
|
||||||
|
Detection prediction_truth = Detection(9.821, -15, 0);
|
||||||
|
|
||||||
|
CHECK_THAT(prediction.get_delay().front(),
|
||||||
|
Catch::Matchers::WithinAbs(prediction_truth.get_delay().front(), 0.01));
|
||||||
|
CHECK_THAT(prediction.get_doppler().front(),
|
||||||
|
Catch::Matchers::WithinAbs(prediction_truth.get_doppler().front(), 0.01));
|
||||||
}
|
}
|
Loading…
Reference in a new issue