FHWS/FtmPrologic
Archived
4
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Added gen7 NUCs and path 10/11

Browse Source
This commit is contained in:
Markus Bullmann
2019-11-13 16:16:53 +01:00
parent ac824937aa
commit 9700f88449
3 changed files with 104 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

80
code/main.cpp
View File

@@ -255,9 +255,9 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
Offline::FileReader fr(setup.training[walkIdx], setup.HasNanoSecondTimestamps);
// ground truth
std::vector<int> gtPath = setup.gtPath;
std::vector<int> gtPath = setup.gtPath;
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
CombinedStats<float> errorStats;
@@ -281,14 +281,14 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
// error file
const long int t = static_cast<long int>(time(NULL));
auto evalDir = std::filesystem::path(Settings::errorDir);
evalDir.append(folder);
evalDir.append(folder);
if (!std::filesystem::exists(evalDir)) {
std::filesystem::create_directory(evalDir);
}
}
std::ofstream errorFile;
errorFile.open (evalDir.string() + "/" + std::to_string(walkIdx) + "_" + std::to_string(t) + ".csv");
errorFile.open(evalDir.string() + "/" + std::to_string(walkIdx) + "_" + std::to_string(t) + ".csv");
// Output dir
auto outputDir = std::filesystem::path(Settings::outputDir);
@@ -300,10 +300,11 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
// wifi
auto kalmanMap = std::make_shared<std::unordered_map<MACAddress, Kalman>>();
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) });
for (auto& nucConfig : setup.NUCs)
{
kalmanMap->insert({ nucConfig.first, Kalman(nucConfig.second.ID, nucConfig.second.kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) });
}
std::cout << "Optimal wifi parameters for " << setup.training[walkIdx] << "\n";
optimizeWifiParameters(fr, gtInterpolator);
@@ -328,10 +329,10 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
plot.setGroundTruth(gtPath);
plot.setView(30, 0);
// APs Positions
plot.addCircle(100000 + 0, Settings::CurrentPath.nucInfo(0).position.xy(), 0.1);
plot.addCircle(100000 + 1, Settings::CurrentPath.nucInfo(1).position.xy(), 0.1);
plot.addCircle(100000 + 2, Settings::CurrentPath.nucInfo(2).position.xy(), 0.1);
plot.addCircle(100000 + 3, Settings::CurrentPath.nucInfo(3).position.xy(), 0.1);
for (auto& nucConfig : setup.NUCs)
{
plot.addCircle(10000 + nucConfig.second.ID, nucConfig.second.position.xy(), 0.1);
}
plot.plot();
// particle-filter
@@ -376,10 +377,10 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
std::unordered_map<MACAddress, MovingMedianTS2> movMedianPerAP;
movMedianPerAP[Settings::NUC1] = MovingMedianTS2(Timestamp::fromMS(500));
movMedianPerAP[Settings::NUC2] = MovingMedianTS2(Timestamp::fromMS(500));
movMedianPerAP[Settings::NUC3] = MovingMedianTS2(Timestamp::fromMS(500));
movMedianPerAP[Settings::NUC4] = MovingMedianTS2(Timestamp::fromMS(500));
for (auto& nucConfig : setup.NUCs)
{
movMedianPerAP[nucConfig.first] = MovingMedianTS2(Timestamp::fromMS(500));
}
for (const Offline::Entry& e : fr.getEntries())
{
@@ -400,10 +401,11 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())).xy();
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
gtDistances.push_back({ gtPos.getDistance(Settings::CurrentPath.nucInfo(0).position.xy()),
gtPos.getDistance(Settings::CurrentPath.nucInfo(1).position.xy()),
gtPos.getDistance(Settings::CurrentPath.nucInfo(2).position.xy()),
gtPos.getDistance(Settings::CurrentPath.nucInfo(3).position.xy()) });
// TODO
//gtDistances.push_back({ gtPos.getDistance(Settings::CurrentPath.nucInfo(0).position.xy()),
// gtPos.getDistance(Settings::CurrentPath.nucInfo(1).position.xy()),
// gtPos.getDistance(Settings::CurrentPath.nucInfo(2).position.xy()),
// gtPos.getDistance(Settings::CurrentPath.nucInfo(3).position.xy()) });
Point3 estPos;
float distErrorFtm = 0;
@@ -450,22 +452,22 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
}
// Store measurements
for (WiFiMeasurement wifi : obs.ftm)
{
if (wifi.getNumSuccessfulMeasurements() < 3)
{
continue;
}
//for (WiFiMeasurement wifi : obs.ftm)
//{
// if (wifi.getNumSuccessfulMeasurements() < 3)
// {
// continue;
// }
Point2 gtPos2 = gtInterpolator.get(static_cast<uint64_t>(wifi.getTimestamp().ms())).xy();
Point2 apPos2 = Settings::CurrentPath.NUCs[wifi.getAP().getMAC()].position.xy();
// Point2 gtPos2 = gtInterpolator.get(static_cast<uint64_t>(wifi.getTimestamp().ms())).xy();
// Point2 apPos2 = Settings::CurrentPath.NUCs[wifi.getAP().getMAC()].position.xy();
float gtDist2 = gtPos2.getDistance(apPos2);
// 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() });
}
// // store distances
// const int nucIdx = Settings::nucIndex(wifi.getAP().getMAC());
// ftmDistances[nucIdx].add(wifi.getTimestamp(), { wifi.getFtmDist(), gtDist2, wifi.getRSSI() }); // TODO
//}
// Kalman debugging (can't be used with active PF)
//{
@@ -692,9 +694,11 @@ int main(int argc, char** argv)
std::vector<Settings::DataSetup> setupsToRun = {
//Settings::data.Path5,
Settings::data.Path7,
Settings::data.Path8,
//Settings::data.Path9
//Settings::data.Path7,
//Settings::data.Path8,
//Settings::data.Path9,
//Settings::data.Path10,
Settings::data.Path11
};
for (Settings::DataSetup setupToRun : setupsToRun)
@@ -702,7 +706,7 @@ int main(int argc, char** argv)
Settings::CurrentPath = setupToRun;
for (size_t walkIdx = 0; walkIdx < 1 /*Settings::CurrentPath.training.size()*/; walkIdx++)
for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++)
{
std::cout << "Executing walk " << walkIdx << "\n";
for (int i = 0; i < 1; ++i)