From 4415288cdaff25f1f1dab30f0d18e6cc0dc14a6e Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 2 Jul 2019 10:36:24 +0200 Subject: [PATCH] Added moving average --- code/FtmKalman.h | 10 +- code/Settings.h | 7 +- code/filter.h | 17 ++-- code/main.cpp | 260 +++++++++++++++++++++++++++++++++++++++++++---- code/mainFtm.cpp | 150 --------------------------- 5 files changed, 263 insertions(+), 181 deletions(-) diff --git a/code/FtmKalman.h b/code/FtmKalman.h index 13ea915..11bdc78 100644 --- a/code/FtmKalman.h +++ b/code/FtmKalman.h @@ -13,6 +13,8 @@ struct Kalman Eigen::Matrix 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) - : nucID(nucID), R(measStdDev*measStdDev) + Kalman(int nucID, float measStdDev, float processNoiseDistance = 0.2, float processNoiseVelocity = 0.4) + : 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, - 0, square(0.3); + Q << square(processNoiseDistance), 0, + 0, square(processNoiseVelocity); // Prediction x = A * x; // Prädizierter Zustand aus Bisherigem und System diff --git a/code/Settings.h b/code/Settings.h index 515a294..fa65053 100644 --- a/code/Settings.h +++ b/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; diff --git a/code/filter.h b/code/filter.h index 8dcf052..a69c0b1 100644 --- a/code/filter.h +++ b/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()); - - pFtm *= Distribution::Normal::getProbability(ftmDist, std::sqrt(kalman.P(0,0)), apDist); - //pFtm *= Distribution::Normal::getProbability(apDist, 3.5, ftmDist); - //pFtm *= Distribution::Region::getProbability(apDist, 3.5/2, ftmDist); - } + if (Settings::UseKalman) + { + auto kalman = kalmanMap->at(wifi.second.getAP().getMAC()); + pFtm *= Distribution::Normal::getProbability(ftmDist, std::sqrt(kalman.P(0,0)), apDist); + } + else + { + pFtm *= Distribution::Normal::getProbability(apDist, 3.5, ftmDist); + //pFtm *= Distribution::Region::getProbability(apDist, 3.5/2, ftmDist); + } + } } diff --git a/code/main.cpp b/code/main.cpp index 6260636..f7c3d89 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -6,6 +6,7 @@ #include "meshPlotter.h" #include "Plotty.h" +#include #include #include #include @@ -32,6 +33,152 @@ using namespace std::chrono_literals; +std::vector> getFtmValues(Offline::FileReader& fr, Interpolator& gtInterpolator, const MACAddress nuc) +{ + std::vector> 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(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 optimizeFtm(std::vector>& values) +{ + std::vector> error; + + for (float offset = 0; offset < 10.0f; offset += 0.25) + { + Stats::Statistics 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 a, std::pair b) { + return a.second < b.second; + }); + + std::cout << "Min ftm offset \t" << minElement->first << "\t" << minElement->second << "\n"; + + return *minElement; +} + +std::pair optimizeRssi(std::vector>& values) +{ + std::vector> error; + + for (float pathLoss = 2.0f; pathLoss < 4.0f; pathLoss += 0.125) + { + Stats::Statistics 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 a, std::pair 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& 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& 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(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 run(Settings::DataSetup setup, int numFile, std::string folder) { // reading file @@ -69,11 +216,13 @@ static Stats::Statistics run(Settings::DataSetup setup, int numFile, std: // wifi auto kalmanMap = std::make_shared>(); - kalmanMap->insert({ Settings::NUC1, Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev) }); - kalmanMap->insert({ Settings::NUC2, Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev) }); - kalmanMap->insert({ Settings::NUC3, Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev) }); - kalmanMap->insert({ Settings::NUC4, Kalman(4, setup.NUCs.at(Settings::NUC4).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, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) }); + 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, 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 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 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 run(Settings::DataSetup setup, int numFile, std: int main(int argc, char** argv) { - mainFtm(argc, argv); - return 0; + //mainFtm(argc, argv); + //return 0; Stats::Statistics statsAVG; @@ -287,16 +458,65 @@ int main(int argc, char** argv) std::string evaluationName = "prologic/tmp"; - 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::vector> error; + std::ofstream error_out; + error_out.open(Settings::errorDir + evaluationName + "_error_path1" + ".csv", std::ios_base::app); - 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 a, std::array 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 a, std::array 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; diff --git a/code/mainFtm.cpp b/code/mainFtm.cpp index b09395d..0e36496 100644 --- a/code/mainFtm.cpp +++ b/code/mainFtm.cpp @@ -29,150 +29,6 @@ #include -using namespace std::chrono_literals; - -std::vector> getFtmValues(Offline::FileReader& fr, Interpolator& gtInterpolator, const MACAddress nuc) -{ - std::vector> 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(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 optimizeFtm(std::vector>& values) -{ - std::vector> error; - - for (float offset = 0; offset < 10.0f; offset += 0.25) - { - Stats::Statistics 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 a, std::pair b) { - return a.second < b.second; - }); - - std::cout << "Min ftm offset \t" << minElement->first << "\t" << minElement->second << "\n"; - - return *minElement; -} - -std::pair optimizeRssi(std::vector>& values) -{ - std::vector> error; - - for (float pathLoss = 2.0f; pathLoss < 4.0f; pathLoss += 0.125) - { - Stats::Statistics 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 a, std::pair 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& 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& 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(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 run(Settings::DataSetup setup, int numFile, std::string folder) { // reading file @@ -276,12 +132,6 @@ static Stats::Statistics 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()) {