Added aggregate methods

code/main.cpp

@@ -34,6 +34,63 @@
 
 using namespace std::chrono_literals;
 
+enum class AggregateMethod {
+    None,
+    Median,
+    MovingMedian
+};
+
+struct MovingMedianTS2
+{
+private:
+    struct TimeValue {
+        Timestamp timestamp;
+        double value;
+    };
+
+    int timeWindow; // ms
+    std::vector<TimeValue> values;
+    
+
+public:
+    MovingMedianTS2() 
+        : timeWindow(0) 
+    {}
+
+    MovingMedianTS2(const Timestamp window)
+        : timeWindow(window.ms())
+    {}
+
+    void add(Timestamp ts, double value)
+    {
+        values.push_back(TimeValue{ ts, value });
+    }
+
+    bool tryGet(const Timestamp ts, double& value)
+    {
+        int wnd = timeWindow;
+
+        values.erase(std::remove_if(values.begin(),
+                                    values.end(),
+                                    [wnd, ts](TimeValue tv) { return std::abs((ts - tv.timestamp).ms()) >= wnd; }),
+                     values.end());
+
+        if (values.size() == 0)
+            return false;
+
+        Stats::Median<double> median;
+
+        for (auto tv : values)
+        {
+            median.add(tv.value);
+        }
+
+        value = median.get();
+        return true;
+    }
+};
+
+
 std::vector<std::tuple<float, float, float>> getFtmValues(Offline::FileReader& fr, Interpolator<uint64_t, Point3>& gtInterpolator, const MACAddress nuc)
 {
     std::vector<std::tuple<float, float, float>> result;
@@ -293,6 +350,13 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
     Plotta::Plotta errorPlot("errorPlot", outputDir.string() + "/errorData.py");
     Plotta::Plotta distsPlot("distsPlot", outputDir.string() + "/distances.py");
 
+
+    std::unordered_map<MACAddress, MovingMedianTS2> movMedianPerAP;
+    movMedianPerAP[Settings::NUC1] = MovingMedianTS2(Timestamp::fromMS(500));
+    movMedianPerAP[Settings::NUC2] = MovingMedianTS2(Timestamp::fromMS(500));
+    movMedianPerAP[Settings::NUC3] = MovingMedianTS2(Timestamp::fromMS(500));
+    movMedianPerAP[Settings::NUC4] = MovingMedianTS2(Timestamp::fromMS(500));
+
     for (const Offline::Entry& e : fr.getEntries())
     {
         if (e.type != Offline::Sensor::WIFI_FTM) {
@@ -321,6 +385,47 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
             float distErrorFtm = 0;
             float distErrorRssi = 0;
 
+            const AggregateMethod aggrMethod = AggregateMethod::None;
+
+            if (aggrMethod == AggregateMethod::Median)
+            {
+                // Compute median of observations
+                std::unordered_map<MACAddress, Stats::Median<double>> apMeas;
+
+                for (WiFiMeasurement wifi : obs.ftm)
+                {
+                    apMeas[wifi.getAP().getMAC()].add(wifi.getFtmDist());
+                }
+
+                obs.ftm.clear();
+
+                for (auto& pair : apMeas)
+                {
+                    double median = pair.second.get();
+
+                    obs.ftm.push_back(WiFiMeasurement(AccessPoint(pair.first), NAN, ts, median, NAN, 3, 3));
+                }
+            }
+            else if (aggrMethod == AggregateMethod::MovingMedian)
+            {
+                for (WiFiMeasurement wifi : obs.ftm)
+                {
+                    movMedianPerAP[wifi.getAP().getMAC()].add(wifi.getTimestamp(), wifi.getFtmDist());
+                }
+
+                obs.ftm.clear();
+
+                for (auto& pair : movMedianPerAP)
+                {
+                    double median = 0;
+                    if (pair.second.tryGet(ts, median))
+                    {
+                        obs.ftm.push_back(WiFiMeasurement(AccessPoint(pair.first), NAN, ts, median, NAN, 3, 3));
+                    }
+                }
+            }
+
+
             // Run PF
             obs.currentTime = ts;
             ctrl.currentTime = ts;