mirror of
https://github.com/30hours/blah2.git
synced 2024-11-08 12:25:42 +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 (CMAKE_RUNTIME_OUTPUT_DIRECTORY "${PROJECT_ROOT}/bin")
|
||||
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 test path: ${PROJECT_BINARY_TEST_DIR}")
|
||||
|
|
|
@ -186,14 +186,15 @@ int main(int argc, char **argv)
|
|||
|
||||
// setup process tracker
|
||||
uint8_t m, n, nDelete;
|
||||
double maxAcc, rangeRes;
|
||||
double maxAcc, rangeRes, lambda;
|
||||
std::string smooth;
|
||||
tree["process"]["tracker"]["initiate"]["M"] >> m;
|
||||
tree["process"]["tracker"]["initiate"]["N"] >> n;
|
||||
tree["process"]["tracker"]["delete"] >> nDelete;
|
||||
tree["process"]["tracker"]["initiate"]["maxAcc"] >> maxAcc;
|
||||
rangeRes = 299792458.0/fs;
|
||||
Tracker *tracker = new Tracker(m, n, nDelete, tCpi, maxAcc, rangeRes);
|
||||
rangeRes = 299792458.0/fs;
|
||||
lambda = 299792458.0/fc;
|
||||
Tracker *tracker = new Tracker(m, n, nDelete, ambiguity->cpi_length_seconds(), maxAcc, rangeRes, lambda);
|
||||
|
||||
// setup process spectrum analyser
|
||||
double spectrumBandwidth = 2000;
|
||||
|
|
|
@ -2,7 +2,8 @@
|
|||
#include <iostream>
|
||||
|
||||
// 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;
|
||||
n = _n;
|
||||
|
@ -11,6 +12,7 @@ Tracker::Tracker(uint32_t _m, uint32_t _n, uint32_t _nDelete, double _cpi, doubl
|
|||
maxAccInit = _maxAccInit;
|
||||
timestamp = 0;
|
||||
rangeRes = _rangeRes;
|
||||
lambda = _lambda;
|
||||
|
||||
double resolutionAcc = 1/(cpi*cpi);
|
||||
uint16_t nAcc = (int)maxAccInit/resolutionAcc;
|
||||
|
@ -67,12 +69,8 @@ void Tracker::update(Detection *detection, uint64_t current)
|
|||
{
|
||||
// predict next position
|
||||
Detection detectionCurrent = track.get_current(i);
|
||||
double delayTrack = detectionCurrent.get_delay().front();
|
||||
double dopplerTrack = detectionCurrent.get_doppler().front();
|
||||
acc = track.get_acceleration(i);
|
||||
delayPredict = delayTrack+((dopplerTrack*T)+(0.5*acc*T*T))/rangeRes;
|
||||
dopplerPredict = dopplerTrack+(acc*T);
|
||||
Detection prediction(delayPredict, dopplerPredict, 0);
|
||||
Detection prediction = predict(detectionCurrent, acc, T);
|
||||
|
||||
// loop over detections to associate
|
||||
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]);
|
||||
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);
|
||||
doNotInitiate[j] = true;
|
||||
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)
|
||||
{
|
||||
std::vector<double> delay = detection->get_delay();
|
||||
|
|
|
@ -6,6 +6,7 @@
|
|||
/// @author 30hours
|
||||
/// @todo Add smoothing capability.
|
||||
/// @todo Fix units up.
|
||||
/// @todo I don't think I callback the true CPI time from ambiguity.
|
||||
|
||||
#ifndef TRACKER_H
|
||||
#define TRACKER_H
|
||||
|
@ -35,6 +36,9 @@ private:
|
|||
/// @brief Range resolution for kinematics equations (m).
|
||||
double rangeRes;
|
||||
|
||||
/// @brief Wavelength for kinematics equations (m).
|
||||
double lambda;
|
||||
|
||||
/// @brief Acceleration values to initiate track (Hz/s).
|
||||
std::vector<double> accInit;
|
||||
|
||||
|
@ -49,11 +53,16 @@ private:
|
|||
|
||||
public:
|
||||
/// @brief Constructor.
|
||||
/// @param delayMin Minimum clutter filter delay (bins).
|
||||
/// @param delayMax Maximum clutter filter delay (bins).
|
||||
/// @param nSamples Number of samples per CPI.
|
||||
/// @param m Track initiation constant for M of N detections.
|
||||
/// @param n Track initiation constant for M of N detections.
|
||||
/// @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.
|
||||
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.
|
||||
/// @return Void.
|
||||
|
@ -71,6 +80,13 @@ public:
|
|||
/// @return Void.
|
||||
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.
|
||||
/// @param detection Detection data for last CPI.
|
||||
/// @return Void.
|
||||
|
|
|
@ -3,6 +3,7 @@
|
|||
/// @author 30hours
|
||||
|
||||
#include <catch2/catch_test_macros.hpp>
|
||||
#include <catch2/matchers/catch_matchers_floating_point.hpp>
|
||||
|
||||
#include "data/Detection.h"
|
||||
#include "data/Track.h"
|
||||
|
@ -17,13 +18,17 @@
|
|||
/// @details Check constructor parameters created correctly.
|
||||
TEST_CASE("Constructor", "[constructor]")
|
||||
{
|
||||
uint32_t m = 3;
|
||||
uint32_t n = 5;
|
||||
uint32_t nDelete = 5;
|
||||
double cpi = 0.5;
|
||||
double maxAccInit = 10;
|
||||
double rangeRes = 100;
|
||||
Tracker tracker = Tracker(m, n, nDelete, cpi, maxAccInit, rangeRes);
|
||||
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);
|
||||
}
|
||||
|
||||
/// @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 n = 5;
|
||||
uint32_t nDelete = 5;
|
||||
double cpi = 0.5;
|
||||
double cpi = 1;
|
||||
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,
|
||||
cpi, maxAccInit, rangeRes);
|
||||
cpi, maxAccInit, rangeRes, lambda);
|
||||
|
||||
|
||||
// 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> doppler = {-20};
|
||||
std::vector<double> doppler = {-20,-15,-10,-5,0,5,10,15,20,25};
|
||||
|
||||
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