diff --git a/.gitignore b/.gitignore index 6bab591..4d6d7f8 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ build/ -measurements/error \ No newline at end of file +measurements/error +output/ \ No newline at end of file diff --git a/code/Settings.h b/code/Settings.h index 3b948f6..8a4934a 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -13,6 +13,7 @@ namespace Settings { const std::string outputDir = "../output/"; const bool UseKalman = false; + const bool UseRSSI = false; /** describes one dataset (map, training, parameter-estimation, ...) */ @@ -465,6 +466,6 @@ namespace Settings { }; } data; - static DataSetup CurrentPath = data.Path11; + static DataSetup CurrentPath = data.Path22; } diff --git a/code/filter.h b/code/filter.h index 4e42683..4cc834a 100644 --- a/code/filter.h +++ b/code/filter.h @@ -288,7 +288,7 @@ struct MyPFEval : public SMC::ParticleFilterEvaluation { kalmanFilters = nullptr; } - double prob = ftmEval(SensorMode::FTM, observation.currentTime, p.state.pos.pos, observation.ftm, kalmanFilters); + double prob = ftmEval(Settings::UseRSSI ? SensorMode::RSSI : SensorMode::FTM, observation.currentTime, p.state.pos.pos, observation.ftm, kalmanFilters); if (assignProps) p.weight = prob; diff --git a/code/main.cpp b/code/main.cpp index 2b7386a..9679384 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -526,14 +526,19 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str 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; + case 6: color = K::GnuplotColor::fromRGB(0, 255, 255); break; 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); + float plotDist = wifi2.getFtmDist(); + if (Settings::UseRSSI) + { + float pathLoss = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).rssi_pathloss; + float rssiDist = LogDistanceModel::rssiToDistance(-40, pathLoss, wifi2.getRSSI()); + plotDist = rssiDist; + } + + plot.addDistanceCircle(apPos.xy(), plotDist, color); } @@ -561,6 +566,7 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str plot.showParticles(pf.getParticles()); plot.setCurEst(estPos); plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); + plot.setTitle(Settings::UseRSSI ? "RSSI" : "FTM"); plot.addEstimationNode(estPos); //plot.setActivity((int)act.get()); @@ -569,10 +575,14 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str //plot.splot.getView().setEqualXY(true); plot.plot(); - std::this_thread::sleep_for(100ms); + //std::this_thread::sleep_for(100ms); } } + + //std::cin.get(); + + printErrorStats(errorStats); std::ofstream plot_out; @@ -752,8 +762,6 @@ int main(int argc, char** argv) - - //std::vector> error; //std::ofstream error_out; //error_out.open(Settings::errorDir + evaluationName + "_error_path1" + ".csv", std::ios_base::app); diff --git a/code/mainProb.cpp b/code/mainProb.cpp index c862d59..d7feaf5 100644 --- a/code/mainProb.cpp +++ b/code/mainProb.cpp @@ -119,7 +119,7 @@ static void plotDensity(Plotty& plot, std::vector>& de for (auto it = density.begin(); ; std::advance(it, 2)) { - if (it > density.end()) + if (it >= density.end()) break; auto p = *it; @@ -179,28 +179,15 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str plot.plot(); // 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) - }; - - std::array apPositions{ - Settings::CurrentPath.NUCs.at(Settings::NUC1).position.xy(), - Settings::CurrentPath.NUCs.at(Settings::NUC2).position.xy(), - Settings::CurrentPath.NUCs.at(Settings::NUC3).position.xy(), - Settings::CurrentPath.NUCs.at(Settings::NUC4).position.xy(), - }; std::vector obs; Timestamp lastTimestamp = Timestamp::fromMS(0); - const float sigma = 3.5; - const int movAvgWnd = 15; - std::array, 4> movAvgsFtm{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} }; - std::array, 4> movAvgsRssi{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} }; + //const float sigma = 3.5; + //const int movAvgWnd = 15; + //std::array, 4> movAvgsFtm{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} }; + //std::array, 4> movAvgsRssi{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} }; std::vector errorValuesFtm, errorValuesRssi; std::vector timestamps; diff --git a/code/misc.h b/code/misc.h index 535d0c0..7bd1513 100644 --- a/code/misc.h +++ b/code/misc.h @@ -52,10 +52,10 @@ template void printErrorStats(std::ostream& stream, const CombinedStats& errorStats) { stream << "Walk error:" << "\n"; - stream << "[m] " << std::setw(10) << "mean" << std::setw(10) << "stdDev" << std::setw(10) << "median" << "\n"; + stream << "[m] " << std::setw(10) << "mean" << std::setw(10) << "stdDev" << std::setw(10) << "median" << std::setw(10) << "count" << "\n"; - stream << "FTM " << std::setw(10) << errorStats.ftm.getAvg() << std::setw(10) << errorStats.ftm.getStdDev() << std::setw(10) << errorStats.ftm.getMedian() << "\n"; - stream << "RSSI " << std::setw(10) << errorStats.rssi.getAvg() << std::setw(10) << errorStats.rssi.getStdDev() << std::setw(10) << errorStats.rssi.getMedian() << "\n"; + stream << "FTM " << std::setw(10) << errorStats.ftm.getAvg() << std::setw(10) << errorStats.ftm.getStdDev() << std::setw(10) << errorStats.ftm.getMedian() << std::setw(10) << errorStats.ftm.getCount() << "\n"; + stream << "RSSI " << std::setw(10) << errorStats.rssi.getAvg() << std::setw(10) << errorStats.rssi.getStdDev() << std::setw(10) << errorStats.rssi.getMedian() << std::setw(10) << errorStats.rssi.getCount() << "\n"; stream << std::endl; }