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