Work in progress on TestTracker updates

CMakeLists.txt

@@ -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}")

src/blah2.cpp

@@ -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);
+  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;

src/process/tracker/Tracker.cpp

@@ -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();

src/process/tracker/Tracker.h

@@ -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.

test/unit/process/tracker/TestTracker.cpp

@@ -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));
+}