Added moving average

code/FtmKalman.h

@@ -13,6 +13,8 @@ struct Kalman
     Eigen::Matrix<float, 2, 2> P; // Covariance
 
     float R = 30;  //  measurement noise covariance   
+    float processNoiseDistance; // stdDev
+    float processNoiseVelocity; // stdDev
 
     float lastTimestamp = NAN; // in sec
 
@@ -22,8 +24,8 @@ struct Kalman
         : nucID(nucID)
     {}
 
-    Kalman(int nucID, float measStdDev)
+    Kalman(int nucID, float measStdDev, float processNoiseDistance = 0.2, float processNoiseVelocity = 0.4)
-        : nucID(nucID), R(measStdDev*measStdDev)
+        : nucID(nucID), R(measStdDev*measStdDev), processNoiseDistance(processNoiseDistance), processNoiseVelocity(processNoiseVelocity)
     {}
 
     float predict(const Timestamp timestamp, const float measurment)
@@ -52,8 +54,8 @@ struct Kalman
              0, 1;
 
         Eigen::Matrix2f Q;  // Process Noise Covariance
-        Q << 0, 0,
+        Q << square(processNoiseDistance), 0,
-             0, square(0.3);
+            0, square(processNoiseVelocity);
 
         // Prediction
         x = A * x;                    // Pr<50>dizierter Zustand aus Bisherigem und System

code/Settings.h

@@ -93,6 +93,8 @@ namespace Settings {
 	const std::string dataDir = "../measurements/data/";
 	const std::string errorDir = "../measurements/error/";
     
+    const bool UseKalman = false;
+
     /** describes one dataset (map, training, parameter-estimation, ...) */
 
     const MACAddress NUC1("38:de:ad:6d:77:25");
@@ -107,6 +109,9 @@ namespace Settings {
         float ftm_offset = 0.0f;
         float rssi_pathloss = 0.0f;
         float kalman_measStdDev = 0.0f;
+
+        float kalman_procNoiseDistStdDev = 0.0f;  // standard deviation of distance for process noise 
+        float kalman_procNoiseVelStdDev = 0.0f;   // standard deviation of velocity for process noise 
     };
 
     struct DataSetup {
@@ -224,7 +229,7 @@ namespace Settings {
             { 0, 1, 2, 11, 10, 9, 10, 11, 2, 6, 5, 12, 13, 12, 5, 6, 7, 8 }
         };
 
-        const DataSetup CurrentPath = Path3;
+        const DataSetup CurrentPath = Path5;
 
     } data;
 

code/filter.h

@@ -318,12 +318,17 @@ public:
                     particlePos.z = 1.3; // smartphone h<>he
                     float apDist = particlePos.getDistance(apPos);
 
-                    auto kalman = kalmanMap->at(wifi.second.getAP().getMAC());
+                    if (Settings::UseKalman)
-                    
+                    {
-                    pFtm *= Distribution::Normal<float>::getProbability(ftmDist, std::sqrt(kalman.P(0,0)), apDist);
+                        auto kalman = kalmanMap->at(wifi.second.getAP().getMAC());
-                    //pFtm *= Distribution::Normal<float>::getProbability(apDist, 3.5, ftmDist);
+                        pFtm *= Distribution::Normal<float>::getProbability(ftmDist, std::sqrt(kalman.P(0,0)), apDist);
-                    //pFtm *= Distribution::Region<float>::getProbability(apDist, 3.5/2, ftmDist);
+                    }
-                }
+                    else
+                    {
+                        pFtm *= Distribution::Normal<float>::getProbability(apDist, 3.5, ftmDist);
+                        //pFtm *= Distribution::Region<float>::getProbability(apDist, 3.5/2, ftmDist);
+                    }
+                                    }
             }
 
             

code/main.cpp

@@ -6,6 +6,7 @@
 #include "meshPlotter.h"
 #include "Plotty.h"
 
+#include <array>
 #include <memory>
 #include <thread>
 #include <filesystem>
@@ -32,6 +33,152 @@
 
 using namespace std::chrono_literals;
 
+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;
+
+    for (const Offline::Entry& e : fr.getEntries())
+    {
+        if (e.type == Offline::Sensor::WIFI_FTM)
+        {
+            const Timestamp ts = Timestamp::fromMS(e.ts);
+
+            Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())) + Point3(0, 0, 1.3);
+
+            auto wifi = fr.getWifiFtm()[e.idx].data;
+
+            if (wifi.getAP().getMAC() == nuc)
+            {
+                Point3 apPos = Settings::data.CurrentPath.NUCs.find(wifi.getAP().getMAC())->second.position;
+                float apDist = gtPos.getDistance(apPos);
+                float ftmDist = wifi.getFtmDist();
+                float rssi = wifi.getRSSI();
+
+                result.push_back({ apDist, ftmDist, rssi });
+            }
+        }
+    }
+
+    return result;
+}
+
+std::pair<float, float> optimizeFtm(std::vector<std::tuple<float, float, float>>& values)
+{
+    std::vector<std::pair<float, float>> error;
+
+    for (float offset = 0; offset < 10.0f; offset += 0.25)
+    {
+        Stats::Statistics<float> diffs;
+
+        for (const auto& data : values)
+        {
+            float apDist = std::get<0>(data);
+            float ftmDist = std::get<1>(data);
+            ftmDist += offset;
+
+            float diff = (apDist - ftmDist);
+
+            diffs.add(diff);
+        }
+
+        error.push_back({ offset, diffs.getSquaredSumAvg() });
+    }
+
+    auto minElement = std::min_element(error.begin(), error.end(), [](std::pair<float, float> a, std::pair<float, float> b) {
+        return a.second < b.second;
+    });
+
+    std::cout << "Min ftm offset \t" << minElement->first << "\t" << minElement->second << "\n";
+
+    return *minElement;
+}
+
+std::pair<float, float> optimizeRssi(std::vector<std::tuple<float, float, float>>& values)
+{
+    std::vector<std::pair<float, float>> error;
+
+    for (float pathLoss = 2.0f; pathLoss < 4.0f; pathLoss += 0.125)
+    {
+        Stats::Statistics<float> diffs;
+
+        for (const auto& data : values)
+        {
+            float apDist = std::get<0>(data);
+            float rssi = std::get<2>(data);
+            float rssiDist = LogDistanceModel::rssiToDistance(-40, pathLoss, rssi);
+
+            float diff = (apDist - rssiDist);
+
+            diffs.add(diff);
+        }
+
+        error.push_back({ pathLoss, diffs.getSquaredSumAvg() });
+    }
+
+    auto minElement = std::min_element(error.begin(), error.end(), [](std::pair<float, float> a, std::pair<float, float> b) {
+        return a.second < b.second;
+    });
+
+    std::cout << "Min path loss \t" << minElement->first << "\t" << minElement->second << "\n";
+
+    return *minElement;
+}
+
+void optimizeWifiParameters(Offline::FileReader& fr, Interpolator<uint64_t, Point3>& gtInterpolator)
+{
+    int i = 1;
+    for (auto nuc : { Settings::NUC1, Settings::NUC2, Settings::NUC3, Settings::NUC4 })
+    {
+        auto values = getFtmValues(fr, gtInterpolator, nuc);
+        std::cout << "NUC" << i++ << "\n";
+
+        optimizeFtm(values);
+        optimizeRssi(values);
+    }
+}
+
+void exportFtmValues(Offline::FileReader& fr, Interpolator<uint64_t, Point3>& gtInterpolator)
+{
+    std::fstream fs;
+    fs.open("test.txt", std::fstream::out);
+
+    fs << "timestamp;nucid;dist;rssiDist;ftmDist;ftmStdDev;numMeas;numSuccesMeas" << "\n";
+
+    for (const Offline::Entry& e : fr.getEntries())
+    {
+        if (e.type == Offline::Sensor::WIFI_FTM)
+        {
+            const Timestamp ts = Timestamp::fromMS(e.ts);
+
+            Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())) + Point3(0, 0, 1.3);
+
+            auto wifi = fr.getWifiFtm()[e.idx].data;
+
+            int nucid = Settings::data.CurrentPath.NUCs.at(wifi.getAP().getMAC()).ID;
+            float ftm_offset = Settings::data.CurrentPath.NUCs.at(wifi.getAP().getMAC()).ftm_offset;
+            float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(wifi.getAP().getMAC()).rssi_pathloss;
+
+            float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI());
+            float ftmDist = wifi.getFtmDist() + ftm_offset; //in m; plus static offset
+            float ftmStdDev = wifi.getFtmDistStd();
+            int numMeas = wifi.getNumAttemptedMeasurements();
+            int numSuccessMeas = wifi.getNumSuccessfulMeasurements();
+
+            Point3 apPos = Settings::data.CurrentPath.NUCs.find(wifi.getAP().getMAC())->second.position;
+            float apDist = gtPos.getDistance(apPos);
+
+            fs << ts.ms() << ";" << nucid << ";" << apDist << ";" << rssiDist << ";" << ftmDist << ";" << ftmStdDev << ";" << numMeas << ";" << numSuccessMeas << "\n";
+        }
+
+    }
+    fs.close();
+}
+
+
+
+static float kalman_procNoiseDistStdDev = 1.2f;  // standard deviation of distance for process noise 
+static float kalman_procNoiseVelStdDev = 0.1f;   // standard deviation of velocity for process noise 
+
 static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string folder) {
 
     // reading file
@@ -69,11 +216,13 @@ static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std:
 
     // wifi
     auto kalmanMap = std::make_shared<std::unordered_map<MACAddress, Kalman>>();
-    kalmanMap->insert({ Settings::NUC1, Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev) });
+    kalmanMap->insert({ Settings::NUC1, Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) });
-    kalmanMap->insert({ Settings::NUC2, Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev) });
+    kalmanMap->insert({ Settings::NUC2, Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) });
-    kalmanMap->insert({ Settings::NUC3, Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev) });
+    kalmanMap->insert({ Settings::NUC3, Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) });
-    kalmanMap->insert({ Settings::NUC4, Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev) });
+    kalmanMap->insert({ Settings::NUC4, Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) });
 
+    std::cout << "Optimal wifi parameters for " << setup.training[numFile] << "\n";
+    optimizeWifiParameters(fr, gtInterpolator);
 
     // mesh
     NM::NavMeshSettings set;
@@ -148,12 +297,35 @@ static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std:
             float ftm_offset = Settings::data.CurrentPath.NUCs.at(ftm.getAP().getMAC()).ftm_offset;
             float ftmDist = ftm.getFtmDist() + ftm_offset; // in m; plus static offset
 
-            auto& kalman = kalmanMap->at(ftm.getAP().getMAC());
-            float predictDist = kalman.predict(ts, ftmDist);
 
-            ftm.setFtmDist(predictDist);
+            if (Settings::UseKalman)
+            {
+                auto& kalman = kalmanMap->at(ftm.getAP().getMAC());
+                float predictDist = kalman.predict(ts, ftmDist);
 
-            obs.wifi.insert_or_assign(ftm.getAP().getMAC(), ftm);
+                ftm.setFtmDist(predictDist);
+
+                obs.wifi.insert_or_assign(ftm.getAP().getMAC(), ftm);
+            }
+            else
+            {
+                // MOV AVG
+                if (obs.wifi.count(ftm.getAP().getMAC()) == 0)
+                {
+                    obs.wifi.insert_or_assign(ftm.getAP().getMAC(), ftm);
+                }
+                else
+                {
+                    auto currFtm = obs.wifi.find(ftm.getAP().getMAC());
+
+                    float currDist = currFtm->second.getFtmDist();
+
+                    const float alpha = 0.6;
+                    float newDist = alpha * currDist + (1 - alpha) * ftmDist;
+
+                    currFtm->second.setFtmDist(newDist);
+                }
+            }
         } else if (e.type == Offline::Sensor::WIFI) {
             //obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
             //ctrl.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
@@ -256,7 +428,6 @@ static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std:
     }
 
 
-
     // get someting on console
     std::cout << "Statistical Analysis Filtering: " << std::endl;
     std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << " Std: " << errorStats.getStdDev() << std::endl;
@@ -275,8 +446,8 @@ static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std:
 
 int main(int argc, char** argv) 
 {
-    mainFtm(argc, argv);
+    //mainFtm(argc, argv);
-    return 0;
+    //return 0;
 
 
     Stats::Statistics<float> statsAVG;
@@ -287,16 +458,65 @@ int main(int argc, char** argv)
 
     std::string evaluationName = "prologic/tmp";
 
-    for(int i = 0; i < 2; ++i){
+    std::vector<std::array<float, 3>> error;
+    std::ofstream error_out;
+    error_out.open(Settings::errorDir + evaluationName + "_error_path1" +  ".csv", std::ios_base::app);
 
-        tmp = run(Settings::data.CurrentPath, 0, evaluationName);
-        statsMedian.add(tmp.getMedian());
-        statsAVG.add(tmp.getAvg());
-        statsSTD.add(tmp.getStdDev());
-        statsQuantil.add(tmp.getQuantile(0.75));       
 
-        std::cout << "Iteration " << i << " completed" << std::endl;
+    //for (kalman_procNoiseDistStdDev = 0.8f; kalman_procNoiseDistStdDev < 1.5f; kalman_procNoiseDistStdDev += 0.1f)
-    }
+    //{
+    //    for (kalman_procNoiseVelStdDev = 0.1f; kalman_procNoiseVelStdDev < 0.5f; kalman_procNoiseVelStdDev += 0.1f)
+    //    {
+            for (int i = 0; i < 2; ++i) {
+
+                tmp = run(Settings::data.CurrentPath, 0, evaluationName);
+                statsMedian.add(tmp.getMedian());
+                statsAVG.add(tmp.getAvg());
+                statsSTD.add(tmp.getStdDev());
+                statsQuantil.add(tmp.getQuantile(0.75));
+
+                std::cout << kalman_procNoiseDistStdDev << " " << kalman_procNoiseVelStdDev << std::endl;
+                std::cout << "Iteration " << i << " completed" << std::endl;
+
+            }
+
+    //        error.push_back({{ kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev, statsAVG.getAvg() }});
+
+    //        auto minElement = std::min_element(error.begin(), error.end(), [](std::array<float, 3> a, std::array<float, 3> b) {
+    //            return a[2] < b[2];
+    //        });
+
+    //        std::cout << "Current min error " << (*minElement)[2] << "\t Q(0)=\t" << (*minElement)[0] << "\t Q(1)=" << (*minElement)[1] << "\n";
+
+    //        error_out << kalman_procNoiseDistStdDev << ";" << kalman_procNoiseVelStdDev << ";" << statsAVG.getAvg() << std::endl;
+
+    //        // reset stats
+    //        statsAVG.reset();
+    //        statsMedian.reset();
+    //        statsSTD.reset();
+    //        statsQuantil.reset();
+    //    }
+    //}
+
+    //auto minElement = std::min_element(error.begin(), error.end(), [](std::array<float, 3> a, std::array<float, 3> b) {
+    //    return a[2] < b[2];
+    //});
+
+    //std::cout << "Global Min error " << (*minElement)[2] << "\t Q(0)=\t" << (*minElement)[0] << "\t Q(1)=" << (*minElement)[1] << "\n";
+
+    //error_out.close();
+
+
+    //for(int i = 0; i < 2; ++i){
+    //    
+    //    tmp = run(Settings::data.CurrentPath, 0, evaluationName);
+    //    statsMedian.add(tmp.getMedian());
+    //    statsAVG.add(tmp.getAvg());
+    //    statsSTD.add(tmp.getStdDev());
+    //    statsQuantil.add(tmp.getQuantile(0.75));       
+
+    //    std::cout << "Iteration " << i << " completed" << std::endl;
+    //}
 
     std::cout << "==========================================================" << std::endl;
     std::cout << "Average of all statistical data: " << std::endl;

code/mainFtm.cpp

@@ -29,150 +29,6 @@
 
 #include <sys/stat.h>
 
-using namespace std::chrono_literals;
-
-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;
-
-    for (const Offline::Entry& e : fr.getEntries())
-    {
-        if (e.type == Offline::Sensor::WIFI_FTM)
-        {
-            const Timestamp ts = Timestamp::fromMS(e.ts);
-
-            Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())) + Point3(0, 0, 1.3);
-
-            auto wifi = fr.getWifiFtm()[e.idx].data;
-
-            if (wifi.getAP().getMAC() == nuc)
-            {
-                Point3 apPos = Settings::data.CurrentPath.NUCs.find(wifi.getAP().getMAC())->second.position;
-                float apDist = gtPos.getDistance(apPos);
-                float ftmDist = wifi.getFtmDist(); 
-                float rssi = wifi.getRSSI();
-
-                result.push_back({ apDist, ftmDist, rssi });
-            }
-        }
-    }
-
-    return result;
-}
-
-std::pair<float, float> optimizeFtm(std::vector<std::tuple<float, float, float>>& values)
-{
-    std::vector<std::pair<float, float>> error;
-    
-    for (float offset = 0; offset < 10.0f; offset += 0.25)
-    {
-        Stats::Statistics<float> diffs;
-
-        for (const auto& data : values)
-        {
-            float apDist = std::get<0>(data);
-            float ftmDist = std::get<1>(data);
-            ftmDist += offset;
-
-            float diff = (apDist - ftmDist);
-
-            diffs.add(diff);
-        }
-       
-        error.push_back({ offset, diffs.getSquaredSumAvg() });
-    }
-
-    auto minElement = std::min_element(error.begin(), error.end(), [](std::pair<float, float> a, std::pair<float, float> b) {
-        return a.second < b.second;
-    });
-
-    std::cout << "Min ftm offset \t" << minElement->first << "\t" << minElement->second << "\n";
-
-    return *minElement;
-}
-
-std::pair<float, float> optimizeRssi(std::vector<std::tuple<float, float, float>>& values)
-{
-    std::vector<std::pair<float, float>> error;
-
-    for (float pathLoss = 2.0f; pathLoss < 4.0f; pathLoss += 0.125)
-    {
-        Stats::Statistics<float> diffs;
-
-        for (const auto& data : values)
-        {
-            float apDist = std::get<0>(data);
-            float rssi = std::get<2>(data);
-            float rssiDist = LogDistanceModel::rssiToDistance(-40, pathLoss, rssi);
-
-            float diff = (apDist - rssiDist);
-
-            diffs.add(diff);
-        }
-
-        error.push_back({ pathLoss, diffs.getSquaredSumAvg() });
-    }
-
-    auto minElement = std::min_element(error.begin(), error.end(), [](std::pair<float, float> a, std::pair<float, float> b) {
-        return a.second < b.second;
-    });
-
-    std::cout << "Min path loss \t" << minElement->first << "\t" << minElement->second << "\n";
-
-    return *minElement;
-}
-
-void optimize(Offline::FileReader& fr, Interpolator<uint64_t, Point3>& gtInterpolator)
-{
-    int i = 1;
-    for (auto nuc : {Settings::NUC1, Settings::NUC2, Settings::NUC3, Settings::NUC4})
-    {
-        auto values = getFtmValues(fr, gtInterpolator, nuc);
-        std::cout << "NUC" << i++ << "\n";
-
-        optimizeFtm(values);
-        optimizeRssi(values);
-    }
-}
-
-void exportFtmValues(Offline::FileReader& fr, Interpolator<uint64_t, Point3>& gtInterpolator)
-{
-    std::fstream fs;
-    fs.open("test.txt", std::fstream::out);
-
-    fs << "timestamp;nucid;dist;rssiDist;ftmDist;ftmStdDev;numMeas;numSuccesMeas" << "\n";
-
-    for (const Offline::Entry& e : fr.getEntries())
-    {
-        if (e.type == Offline::Sensor::WIFI_FTM)
-        {
-            const Timestamp ts = Timestamp::fromMS(e.ts);
-
-            Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())) + Point3(0, 0, 1.3);
-
-            auto wifi = fr.getWifiFtm()[e.idx].data;
-
-            int nucid = Settings::data.CurrentPath.NUCs.at(wifi.getAP().getMAC()).ID;
-            float ftm_offset = Settings::data.CurrentPath.NUCs.at(wifi.getAP().getMAC()).ftm_offset;
-            float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(wifi.getAP().getMAC()).rssi_pathloss;
-
-            float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI());
-            float ftmDist = wifi.getFtmDist() + ftm_offset; //in m; plus static offset
-            float ftmStdDev = wifi.getFtmDistStd();
-            int numMeas = wifi.getNumAttemptedMeasurements();
-            int numSuccessMeas = wifi.getNumSuccessfulMeasurements();
-
-            Point3 apPos = Settings::data.CurrentPath.NUCs.find(wifi.getAP().getMAC())->second.position;
-            float apDist = gtPos.getDistance(apPos);
-
-            fs << ts.ms() << ";" << nucid << ";" << apDist << ";" << rssiDist << ";" << ftmDist << ";" << ftmStdDev << ";" << numMeas << ";" << numSuccessMeas << "\n";
-        }
-
-    }
-    fs.close();
-
-}
-
 static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string folder) {
 
     // reading file
@@ -276,12 +132,6 @@ static Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std:
     relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
     Timestamp lastTimestamp = Timestamp::fromMS(0);
 
-    //optimize(fr, gtInterpolator);
-    //return errorStats;
-
-    int i = 0;
-    //exportFtmValues(fr, gtInterpolator);
-
 
     // parse each sensor-value within the offline data
     for (const Offline::Entry& e : fr.getEntries()) {