From deb318e115a5db2f4734f21dacbc9e67b199c565 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Wed, 25 Sep 2019 16:49:57 +0200 Subject: [PATCH] Particle filter is now using a fixed time interval as step --- code/Plotta.h | 9 +++ code/Plotty.h | 14 +++++ code/Settings.h | 6 ++ code/filter.h | 80 +++++++++++++++++++----- code/main.cpp | 159 +++++++++++++++++++++++++++--------------------- 5 files changed, 185 insertions(+), 83 deletions(-) diff --git a/code/Plotta.h b/code/Plotta.h index 395de2c..2eb22e6 100644 --- a/code/Plotta.h +++ b/code/Plotta.h @@ -6,6 +6,7 @@ #include #include #include +#include namespace Plotta { @@ -39,6 +40,8 @@ namespace Plotta { // send data std::ofstream stream; + stream.exceptions(stream.exceptions() | std::ios::failbit); + stream.open(dataFile); std::time_t t = std::time(nullptr); @@ -126,6 +129,12 @@ namespace Plotta return writeNumpyArray(stream, list.begin(), list.end()); } + template + plottastream& operator<<(plottastream& stream, const std::array& list) + { + return writeNumpyArray(stream, list.begin(), list.end()); + } + template static plottastream& operator<<(plottastream& stream, const T& value) diff --git a/code/Plotty.h b/code/Plotty.h index a301171..e2ef07c 100644 --- a/code/Plotty.h +++ b/code/Plotty.h @@ -184,6 +184,20 @@ public: splot.getObjects().set(id, obj); + } + + void addCircle(int id, const Point2& center, float radius, const K::GnuplotColor& strokeColor) + { + auto c = K::GnuplotCoordinate2(center.x, center.y, K::GnuplotCoordinateSystem::FIRST); + auto r = K::GnuplotCoordinate1(radius, K::GnuplotCoordinateSystem::FIRST); + + K::GnuplotFill fill(K::GnuplotFillStyle::EMPTY, K::GnuplotColor::fromRGB(0, 0, 0)); + K::GnuplotStroke stroke(K::GnuplotDashtype::SOLID, 1, strokeColor); + + K::GnuplotObjectCircle* obj = new K::GnuplotObjectCircle(c, r, fill, stroke); + + splot.getObjects().set(id, obj); + } void addBBoxes(const BBoxes3& boxes, const K::GnuplotColor& c) { diff --git a/code/Settings.h b/code/Settings.h index cc9cf8c..4887be8 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -94,6 +94,7 @@ namespace Settings { const std::string mapDir = "../map/"; const std::string dataDir = "../measurements/data/"; const std::string errorDir = "../measurements/error/"; + const std::string plotDataDir = "../plots/data/"; const bool UseKalman = true; @@ -147,6 +148,11 @@ namespace Settings { { return NUCs.at(nucFromIndex(idx)); } + + NUCSettings nuc(const MACAddress& mac) const + { + return NUCs.at(mac); + } }; /** all configured datasets */ diff --git a/code/filter.h b/code/filter.h index faf1553..552c209 100644 --- a/code/filter.h +++ b/code/filter.h @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -114,8 +115,8 @@ struct MyObservation { //wifi std::unordered_map wifi; // deprecated - std::array dists; - std::array sigmas; // from kalman + + std::vector ftm; //time Timestamp currentTime; @@ -263,34 +264,85 @@ struct MyPFEval : public SMC::ParticleFilterEvaluation { MyPFEval() { }; bool assignProps = false; + std::shared_ptr> ftmKalmanFilters; virtual double evaluation(std::vector>& particles, const MyObservation& observation) override { double sum = 0; - //#pragma omp parallel for num_threads(3) + #pragma omp parallel for num_threads(3) for (int i = 0; i < particles.size(); ++i) { SMC::Particle& p = particles[i]; double pFtm = 1.0; - for (size_t i = 0; i < 4; i++) + for (WiFiMeasurement wifi : observation.ftm) { - float dist = observation.dists[i]; - const float sigma = isnan(observation.sigmas[i]) ? 3.5 : observation.sigmas[i]; - - if (!isnan(dist)) + if (wifi.getNumSuccessfulMeasurements() < 3) { - Point3 apPos = Settings::data.CurrentPath.nucInfo(i).position; - Point3 particlePos = p.state.pos.pos; - particlePos.z = 1.3; // smartphone höhe - float apDist = particlePos.getDistance(apPos); + continue; + } - double x = Distribution::Normal::getProbability(dist, std::sqrt(sigma), apDist); - pFtm *= x; + const MACAddress& mac = wifi.getAP().getMAC(); + int nucIndex = Settings::nucIndex(mac); + + const Point3 apPos = Settings::data.CurrentPath.nucInfo(nucIndex).position; + Point3 particlePos = p.state.pos.pos; + particlePos.z = 1.3; // smartphone höhe + const float apDist = particlePos.getDistance(apPos); + + // compute ftm distance + float ftm_offset = Settings::data.CurrentPath.NUCs.at(mac).ftm_offset; + float ftmDist = wifi.getFtmDist() + ftm_offset; // in m; plus static offset + + float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(mac).rssi_pathloss; + float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI()); + + + if (ftmDist > 0) + { + double sigma = wifi.getFtmDistStd()*wifi.getFtmDistStd(); // 3.5; // TODO + + if (sigma == 0) + { + sigma = 38*38; + } + + if (Settings::UseKalman) + { + Kalman& kalman = ftmKalmanFilters->at(mac); + ftmDist = kalman.predict(observation.currentTime, ftmDist); + sigma = kalman.P(0, 0); + + //Assert::isTrue(sigma > 0, "sigma"); + if (sigma <= 0) + { + sigma = 38 * 38; + } + + double x = Distribution::Normal::getProbability(ftmDist, std::sqrt(sigma), apDist); + + if (x > 1e-90) + { + pFtm *= x; + Assert::isTrue(pFtm != 0, "zero prob"); + } + } + else + { + double x = Distribution::Normal::getProbability(ftmDist, std::sqrt(sigma), apDist); + + if (x > 1e-90) + { + pFtm *= x; + + Assert::isTrue(pFtm != 0, "zero prob"); + } + } } } + double prob = pFtm; if (assignProps) diff --git a/code/main.cpp b/code/main.cpp index 95a6957..75c4f8b 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -5,6 +5,7 @@ #include "Settings.h" #include "meshPlotter.h" #include "Plotty.h" +#include "Plotta.h" #include #include @@ -216,12 +217,11 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str // wifi - std::array ftmKalmanFilters{ - Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev), - Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev), - Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev), - Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) - }; + auto kalmanMap = std::make_shared>(); + 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[walkIdx] << "\n"; optimizeWifiParameters(fr, gtInterpolator); @@ -257,12 +257,14 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str //auto init = std::make_unique(&mesh, srcPath0); // known position auto init = std::make_unique(&mesh); // uniform distribution auto eval = std::make_unique(); + eval->ftmKalmanFilters = kalmanMap; auto trans = std::make_unique(); //auto trans = std::make_unique(); auto resample = std::make_unique>(); - auto estimate = std::make_unique>(); + //auto estimate = std::make_unique>(); + auto estimate = std::make_unique>(); // setup MyFilter pf(numParticles, std::move(init)); @@ -277,60 +279,51 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str MyObservation obs; Timestamp lastTimestamp = Timestamp::fromMS(0); - - std::vector data = filterOfflineData(fr); - + std::vector errorValuesFtm, errorValuesRssi; std::vector timestamps; + std::vector> gtDistances, ftmDistances, rssiDistances; // distance per AP + Plotta::Plotta errorPlot("errorPlot", Settings::plotDataDir + "errorData.py"); + Plotta::Plotta distsPlot("distsPlot", Settings::plotDataDir + "distances.py"); - for (const WifiMeas& wifi : data) + for (const Offline::Entry& e : fr.getEntries()) { - Point2 gtPos = gtInterpolator.get(static_cast(wifi.ts.ms())).xy(); - plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); + if (e.type != Offline::Sensor::WIFI_FTM) { + continue; + } - Point3 estPos; - float distErrorFtm = 0; - float distErrorRssi = 0; + // TIME + const Timestamp ts = Timestamp::fromMS(e.ts); - // FTM + auto wifiFtm = fr.getWifiFtm()[e.idx].data; + obs.ftm.push_back(wifiFtm); + + + if (ts - lastTimestamp >= Timestamp::fromMS(500)) { - std::array dists = wifi.ftmDists; - std::array sigmas = {NAN, NAN, NAN, NAN }; + // Do update step + Point2 gtPos = gtInterpolator.get(static_cast(ts.ms())).xy(); + plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); - for (size_t i = 0; i < 4; i++) - { - if (dists[i] <= 0) - { - dists[i] = NAN; - } - } + gtDistances.push_back({ gtPos.getDistance(Settings::data.CurrentPath.nucInfo(0).position.xy()), + gtPos.getDistance(Settings::data.CurrentPath.nucInfo(1).position.xy()), + gtPos.getDistance(Settings::data.CurrentPath.nucInfo(2).position.xy()), + gtPos.getDistance(Settings::data.CurrentPath.nucInfo(3).position.xy()) }); - if (Settings::UseKalman) - { - for (size_t i = 0; i < 4; i++) - { - if (!isnan(dists[i])) - { - dists[i] = ftmKalmanFilters[i].predict(wifi.ts, dists[i]); - sigmas[i] = ftmKalmanFilters[i].P(0, 0); - } - } - } - - obs.dists = dists; - obs.sigmas = sigmas; + Point3 estPos; + float distErrorFtm = 0; + float distErrorRssi = 0; // Run PF - obs.currentTime = wifi.ts; - ctrl.currentTime = wifi.ts; + obs.currentTime = ts; + ctrl.currentTime = ts; MyState est = pf.update(&ctrl, obs); ctrl.afterEval(); - lastTimestamp = wifi.ts; + lastTimestamp = ts; estPos = est.pos.pos; - ctrl.lastEstimate = estPos; plot.setCurEst(Point3(estPos.x, estPos.y, 0.1)); @@ -341,35 +334,63 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str errorStats.ftm.add(distErrorFtm); // draw wifi ranges - for (size_t i = 0; i < 4; i++) + for (size_t i = 0; i < obs.ftm.size(); i++) { - Point3 apPos = Settings::data.CurrentPath.nucInfo(i).position; - plot.addCircle(1000+i, apPos.xy(), dists[i]); + WiFiMeasurement wifi2 = obs.ftm[i]; + + Point3 apPos = Settings::data.CurrentPath.nuc(wifi2.getAP().getMAC()).position; + + K::GnuplotColor color; + switch (Settings::data.CurrentPath.nuc(wifi2.getAP().getMAC()).ID) + { + case 1: color = K::GnuplotColor::fromRGB(0, 255, 0); break; + case 2: color = K::GnuplotColor::fromRGB(0, 0, 255); break; + case 3: color = K::GnuplotColor::fromRGB(255, 255, 0); break; + default: color = K::GnuplotColor::fromRGB(255, 0, 0); break; + } + + plot.addCircle(1000 + i, apPos.xy(), wifi2.getFtmDist(), color); } + + obs.wifi.clear(); + obs.ftm.clear(); + + + errorValuesFtm.push_back(distErrorFtm); + errorValuesRssi.push_back(distErrorRssi); + timestamps.push_back(ts.ms()); + + // Error plot + errorPlot.add("t", timestamps); + errorPlot.add("errorFtm", errorValuesFtm); + errorPlot.add("errorRssi", errorValuesRssi); + errorPlot.frame(); + + // Distances plot + //distsPlot.add("t", timestamps); + //distsPlot.add("gtDists", gtDistances); + //distsPlot.add("ftmDists", ftmDistances); + //distsPlot.frame(); + + // Plotting + plot.showParticles(pf.getParticles()); + plot.setCurEst(estPos); + plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); + + plot.addEstimationNode(estPos); + //plot.setActivity((int)act.get()); + //plot.splot.getView().setEnabled(false); + //plot.splot.getView().setCamera(0, 0); + //plot.splot.getView().setEqualXY(true); + + plot.plot(); } - - errorValuesFtm.push_back(distErrorFtm); - errorValuesRssi.push_back(distErrorRssi); - timestamps.push_back(wifi.ts.ms()); - - // Plotting - //plot.showParticles(pf.getParticles()); - plot.setCurEst(estPos); - plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); - - plot.addEstimationNode(estPos); - //plot.setActivity((int)act.get()); - //plot.splot.getView().setEnabled(false); - //plot.splot.getView().setCamera(0, 0); - //plot.splot.getView().setEqualXY(true); - - plot.plot(); - //std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - - + printErrorStats(errorStats); + //system("pause"); + return errorStats; } @@ -398,10 +419,10 @@ int main(int argc, char** argv) std::string evaluationName = "prologic/tmp"; - for (size_t walkIdx = 0; walkIdx < Settings::data.CurrentPath.training.size(); walkIdx++) + for (size_t walkIdx = 0; walkIdx < 1 /*Settings::data.CurrentPath.training.size()*/; walkIdx++) { std::cout << "Executing walk " << walkIdx << "\n"; - for (int i = 0; i < 1; ++i) + for (int i = 0; i < 5; ++i) { std::cout << "Start of iteration " << i << "\n";