From 955e6b366f64943fe3bb2670b975ddb6c4f7a8b4 Mon Sep 17 00:00:00 2001
From: 30hours <nathan@30hours.dev>
Date: Wed, 27 Dec 2023 06:26:58 +0000
Subject: [PATCH] Tracker skeleton complete but still intermittant segfault

---
 src/blah2.cpp                   |  5 +++--
 src/data/Track.h                |  1 +
 src/process/tracker/Tracker.cpp | 24 ++++++++++++++----------
 src/process/tracker/Tracker.h   |  5 ++++-
 4 files changed, 22 insertions(+), 13 deletions(-)

diff --git a/src/blah2.cpp b/src/blah2.cpp
index e06d00d..d7d84b2 100644
--- a/src/blah2.cpp
+++ b/src/blah2.cpp
@@ -185,13 +185,14 @@ int main(int argc, char **argv)
 
   // setup process tracker
   uint8_t m, n, nDelete;
-  double maxAcc;
+  double maxAcc, rangeRes;
   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;
-  Tracker *tracker = new Tracker(m, n, nDelete, tCpi, maxAcc);
+  rangeRes = 299792458.0/fs; 
+  Tracker *tracker = new Tracker(m, n, nDelete, tCpi, maxAcc, rangeRes);
 
   // setup process spectrum analyser
   double spectrumBandwidth = 2000;
diff --git a/src/data/Track.h b/src/data/Track.h
index 87e42ea..3a5a884 100644
--- a/src/data/Track.h
+++ b/src/data/Track.h
@@ -4,6 +4,7 @@
 /// @details The ID is 4 digit hexadecimal with 16^4 = 65536 combinations.
 /// @details The state can be TENTATIVE, ASSOCIATED, ACTIVE or COASTING.
 /// @details Associated detections use null detections when no updates.
+/// @details Current is seperated from associated for smoothing.
 /// @author 30hours
 /// @todo I feel promote() should be implemented in the tracker.
 
diff --git a/src/process/tracker/Tracker.cpp b/src/process/tracker/Tracker.cpp
index 7fe4002..50dd32b 100644
--- a/src/process/tracker/Tracker.cpp
+++ b/src/process/tracker/Tracker.cpp
@@ -2,7 +2,7 @@
 #include <iostream>
 
 // constructor
-Tracker::Tracker(uint32_t _m, uint32_t _n, uint32_t _nDelete, double _cpi, double _maxAccInit)
+Tracker::Tracker(uint32_t _m, uint32_t _n, uint32_t _nDelete, double _cpi, double _maxAccInit, double _rangeRes)
 {
   m = _m;
   n = _n;
@@ -10,8 +10,9 @@ Tracker::Tracker(uint32_t _m, uint32_t _n, uint32_t _nDelete, double _cpi, doubl
   cpi = _cpi;
   maxAccInit = _maxAccInit;
   timestamp = 0;
+  rangeRes = _rangeRes;
 
-  double resolutionAcc = 1 * (1/(cpi*cpi));
+  double resolutionAcc = 1/(cpi*cpi);
   uint16_t nAcc = (int)maxAccInit/resolutionAcc;
   for (int i = 0; i < 2*nAcc+1; i++)
   {
@@ -27,7 +28,6 @@ Tracker::~Tracker()
 
 Track *Tracker::process(Detection *detection, uint64_t currentTime)
 {
-  timestamp = currentTime;
   doNotInitiate.clear();
   for (size_t i = 0; i < detection->get_nDetections(); i++)
   {
@@ -36,7 +36,11 @@ Track *Tracker::process(Detection *detection, uint64_t currentTime)
 
   if (track.get_n() > 0)
   {
-    update(detection, timestamp);
+    update(detection, currentTime);
+  }
+  else
+  {
+    timestamp = currentTime;
   }
   initiate(detection);
 
@@ -50,13 +54,13 @@ void Tracker::update(Detection *detection, uint64_t current)
   std::vector<double> snr = detection->get_snr();
 
   // init
-  double delayPredict, dopplerPredict;
-  double acc;
+  double delayPredict, dopplerPredict, acc;
   uint32_t nRemove = 0;
   std::string state;
 
   // get time between detections
-  double T = (double) current - timestamp;
+  double T = ((double)(current - timestamp))/1000;
+  timestamp = current;
 
   // loop over each track
   for (int i = 0; i < track.get_n(); i++)
@@ -66,10 +70,10 @@ void Tracker::update(Detection *detection, uint64_t current)
     double delayTrack = detectionCurrent.get_delay().front();
     double dopplerTrack = detectionCurrent.get_doppler().front();
     acc = track.get_acceleration(i);
-    delayPredict = delayTrack+(1/(2*3.14))*(dopplerTrack+0.5*acc*T*T);
-    dopplerPredict = dopplerTrack+acc*T;
+    delayPredict = delayTrack+((dopplerTrack*T)+(0.5*acc*T*T))/rangeRes;
+    dopplerPredict = dopplerTrack+(acc*T);
     Detection prediction(delayPredict, dopplerPredict, 0);
-
+    
     // loop over detections to associate
     for (size_t j = 0; j < detection->get_nDetections(); j++)
     {
diff --git a/src/process/tracker/Tracker.h b/src/process/tracker/Tracker.h
index 4a10353..aa0ebd6 100644
--- a/src/process/tracker/Tracker.h
+++ b/src/process/tracker/Tracker.h
@@ -32,6 +32,9 @@ private:
   /// @brief Maximum acceleration to initiate track (Hz/s).
   double maxAccInit;
 
+  /// @brief Range resolution for kinematics equations (m).
+  double rangeRes;
+
   /// @brief Acceleration values to initiate track (Hz/s).
   std::vector<double> accInit;
 
@@ -50,7 +53,7 @@ public:
   /// @param delayMax Maximum clutter filter delay (bins).
   /// @param nSamples Number of samples per CPI.
   /// @return The object.
-  Tracker(uint32_t m, uint32_t n, uint32_t nDelete, double cpi, double maxAccInit);
+  Tracker(uint32_t m, uint32_t n, uint32_t nDelete, double cpi, double maxAccInit, double rangeRes);
 
   /// @brief Destructor.
   /// @return Void.