diff --git a/code/Settings.h b/code/Settings.h index 10f3efc..3f3794e 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -198,10 +198,10 @@ namespace Settings { dataDir + "Pixel2/path5/1560158988785_6_6.csv" }, { - { NUC1, {1, { 8.1, 18.7, 0.8}, 2.00, 3.375, 3.0f} }, // NUC 1 - { NUC2, {2, { 8.4, 27.3, 0.8}, 1.25, 3.375, 3.0f} }, // NUC 2 - { NUC3, {3, {21.3, 19.3, 0.8}, 2.75, 3.250, 3.0f} }, // NUC 3 - { NUC4, {4, {20.6, 26.8, 0.8}, 2.25, 3.375, 3.0f} }, // NUC 4 + { NUC1, {1, { 8.1, 18.7, 0.8}, 0.00, 3.375, 3.0f} }, // NUC 1 + { NUC2, {2, { 8.4, 27.3, 0.8}, 0.00, 3.375, 3.0f} }, // NUC 2 + { NUC3, {3, {21.3, 19.3, 0.8}, 0.00, 3.250, 3.0f} }, // NUC 3 + { NUC4, {4, {20.6, 26.8, 0.8}, 0.00, 3.375, 3.0f} }, // NUC 4 }, { 0, 1, 2, 11, 10, 9, 10, 11, 2, 6, 5, 12, 13, 12, 5, 6, 7, 8 }, false @@ -226,7 +226,7 @@ namespace Settings { true }; - // 7 Path: SHL Path 2; Versuche mit NUCs in den Räumen war nicht vielversprechend ... + // 7 Path: SHL Path 2; Versuche mit NUCs in den Räumen const DataSetup Path7 = { "path7", mapDir + "shl.xml", @@ -287,6 +287,6 @@ namespace Settings { }; } data; - static DataSetup CurrentPath = data.Path7; + static DataSetup CurrentPath = data.Path9; } diff --git a/code/main.cpp b/code/main.cpp index fac014a..74664de 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -232,7 +232,18 @@ void exportFtmValues(Offline::FileReader& fr, Interpolator& gt fs.close(); } +template +struct TimeSeries +{ + std::vector t; + std::vector values; + void add(const Timestamp ts, const T value) + { + t.push_back(ts); + values.push_back(value); + } +}; 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 @@ -252,12 +263,20 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str //calculate distance of path std::vector::InterpolatorEntry> gtEntries = gtInterpolator.getEntries(); - double distance = 0; - for(int i = 1; i < gtEntries.size(); ++i){ - distance += gtEntries[i].value.getDistance(gtEntries[i-1].value); + double gtTotalDistance = 0; + Stats::Statistics gtWalkingSpeed; + for (int i = 1; i < gtEntries.size(); ++i) { + double distance = gtEntries[i].value.getDistance(gtEntries[i - 1].value); + double timeDiff = static_cast(gtEntries[i].key - gtEntries[i - 1].key); + + double walkingSpeed = distance / (timeDiff / 1000.0f); // m / s + + gtWalkingSpeed.add(walkingSpeed); + gtTotalDistance += distance; } - std::cout << "Distance of Path: " << distance << std::endl; + std::cout << "Distance of Path: " << gtTotalDistance << std::endl; + std::cout << "GT walking speed: " << gtWalkingSpeed.getAvg() << "m/s (" << gtWalkingSpeed.getAvg()*3.6f << "km/h)" << std::endl; // error file const long int t = static_cast(time(NULL)); @@ -345,7 +364,12 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str std::vector errorValuesFtm, errorValuesRssi; std::vector timestamps; - std::vector> gtDistances, ftmDistances, rssiDistances; // distance per AP + + std::vector timestampsDist; + std::vector> gtDistances, rssiDistances; // distance per AP + + std::array>, 4> ftmDistances; + TimeSeries> ftmPredictions; Plotta::Plotta errorPlot("errorPlot", outputDir.string() + "/errorData.py"); Plotta::Plotta distsPlot("distsPlot", outputDir.string() + "/distances.py"); @@ -425,6 +449,47 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str } } + // Store measurements + for (WiFiMeasurement wifi : obs.ftm) + { + if (wifi.getNumSuccessfulMeasurements() < 3) + { + continue; + } + + Point2 gtPos2 = gtInterpolator.get(static_cast(wifi.getTimestamp().ms())).xy(); + Point2 apPos2 = Settings::CurrentPath.NUCs[wifi.getAP().getMAC()].position.xy(); + + float gtDist2 = gtPos2.getDistance(apPos2); + + // store distances + const int nucIdx = Settings::nucIndex(wifi.getAP().getMAC()); + ftmDistances[nucIdx].add(wifi.getTimestamp(), { wifi.getFtmDist(), gtDist2, wifi.getRSSI() }); + } + + // Kalman debugging (can't be used with active PF) + //{ + // // Kalman predict & update for available measurments + // for (WiFiMeasurement wifi : obs.ftm) + // { + // kalmanMap->at(wifi.getAP().getMAC()).predictAndUpdate(wifi.getTimestamp(), wifi.getFtmDist()); + // } + + // // Kalman prediction only for current timestamp + // std::array pred; + // for (size_t i = 0; i < 4; i++) + // { + // KalmanPrediction prediction = kalmanMap->at(Settings::nucFromIndex(i)).predict(ts); + // prediction.P[0] = kalmanMap->at(Settings::nucFromIndex(i)).P[0]; + // prediction.P[1] = kalmanMap->at(Settings::nucFromIndex(i)).P[1]; + // prediction.P[2] = kalmanMap->at(Settings::nucFromIndex(i)).P[2]; + // prediction.P[3] = kalmanMap->at(Settings::nucFromIndex(i)).P[3]; + + // pred[i] = prediction; + // } + // ftmPredictions.add(ts, pred); + //} + // Run PF obs.currentTime = ts; @@ -462,9 +527,14 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str default: color = K::GnuplotColor::fromRGB(255, 0, 0); break; } + + //float rssiDist = LogDistanceModel::rssiToDistance(-40, 2.5, wifi2.getRSSI()); + //plot.addDistanceCircle(apPos.xy(), rssiDist, color); + plot.addDistanceCircle(apPos.xy(), wifi2.getFtmDist(), color); } + obs.wifi.clear(); obs.ftm.clear(); @@ -497,6 +567,7 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str //plot.splot.getView().setEqualXY(true); plot.plot(); + std::this_thread::sleep_for(100ms); } } @@ -514,7 +585,84 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str errorPlot.frame(); - //system("pause"); + // MATLAB output + //{ + // std::ofstream matlab_error_out; + // matlab_error_out.open(outputDir.string() + "/error.csv"); + + // matlab_error_out << "t;ftmError" << "\n"; + + // for (size_t i = 0; i < timestamps.size(); i++) + // { + // matlab_error_out << timestamps[i] << ";" << errorValuesFtm[i] << "\n"; + // } + //} + + //{ + // std::ofstream matlab_gt_out; + // matlab_gt_out.open(outputDir.string() + "/distance_gt.csv"); + + // matlab_gt_out << "t;distGT1;distGT2;distGT3;distGT4" << "\n"; + + // for (size_t i = 0; i < gtDistances.size(); i++) + // { + // matlab_gt_out << timestamps[i] << ";" << gtDistances[i][0] << ";" << gtDistances[i][1] << ";" << gtDistances[i][2] << ";" << gtDistances[i][3] << "\n"; + // } + //} + + //for (size_t i = 0; i < 4; i++) + //{ + // std::ofstream matlab_out; + // matlab_out.open(outputDir.string() + "/distance_ap" + std::to_string(i+1) + ".csv"); + + // matlab_out << "t;distAp;distGT;rssi" << "\n"; + + // for (size_t j = 0; j < ftmDistances[i].t.size(); j++) + // { + // matlab_out << ftmDistances[i].t[j].ms() + // << ";" << ftmDistances[i].values[j][0] + // << ";" << ftmDistances[i].values[j][1] + // << ";" << ftmDistances[i].values[j][2] + // << "\n"; + // } + //} + + //{ + // std::ofstream matlab_prediction_out; + // matlab_prediction_out.open(outputDir.string() + "/predictions.csv"); + + // matlab_prediction_out << "t;pAP1d;pAP1dDev;pAP1s;pAP1sDev;pAP2d;pAP2dDev;pAP2s;pAP2sDev;pAP3d;pAP3dDev;pAP3s;pAP3sDev;pAP4d;pAP4dDev;pAP4s;pAP4sDev" << "\n"; + // for (size_t i = 0; i < ftmPredictions.values.size(); i++) + // { + // matlab_prediction_out << ftmPredictions.t[i].ms(); + // for (size_t j = 0; j < 4; j++) + // { + // const KalmanPrediction v = ftmPredictions.values[i][j]; + + // if (isnan(v.distance)) + // matlab_prediction_out << ";nan"; + // else + // matlab_prediction_out << ";" << v.distance; + + // if (isnan(v.P[0])) + // matlab_prediction_out << ";nan"; + // else + // matlab_prediction_out << ";" << std::sqrt(v.P[0]); + + // if (isnan(v.speed)) + // matlab_prediction_out << ";nan"; + // else + // matlab_prediction_out << ";" << v.speed; + + // if (isnan(v.P[2])) + // matlab_prediction_out << ";nan"; + // else + // matlab_prediction_out << ";" << std::sqrt(v.P[2]); + + // } + // matlab_prediction_out << "\n"; + // } + //} return errorStats; } @@ -542,9 +690,12 @@ int main(int argc, char** argv) std::string evaluationName = "prologic/tmp"; - std::vector setupsToRun = { Settings::data.Path7, - Settings::data.Path8, - Settings::data.Path9 }; + std::vector setupsToRun = { + //Settings::data.Path5, + Settings::data.Path7, + Settings::data.Path8, + //Settings::data.Path9 + }; for (Settings::DataSetup setupToRun : setupsToRun) { diff --git a/code/mainProb.cpp b/code/mainProb.cpp index e410f1c..c862d59 100644 --- a/code/mainProb.cpp +++ b/code/mainProb.cpp @@ -237,7 +237,7 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str computeDensity(bbox, density, maxElement, obs, true, 3.5); Point2 estPos = maxElement.first; - plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1)); + //plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1)); plot.setCurEst(Point3(estPos.x, estPos.y, 0.1)); // Plot density @@ -257,7 +257,7 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str computeDensity(bbox, density, maxElement, obs, false, 8); Point2 estPos = maxElement.first; - plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1)); + //plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1)); // Plot density //plotDensity(plot, density); @@ -267,6 +267,26 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str errorStats.rssi.add(distErrorRssi); } + // draw wifi ranges + plot.clearDistanceCircles(); + + for (size_t i = 0; i < obs.size(); i++) + { + WiFiMeasurement wifi2 = obs[i]; + + Point3 apPos = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).position; + + K::GnuplotColor color; + switch (Settings::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.addDistanceCircle(apPos.xy(), wifi2.getFtmDist(), color); + } errorValuesFtm.push_back(distErrorFtm); errorValuesRssi.push_back(distErrorRssi);