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

2
code/Eval.cpp
View File

@@ -60,7 +60,7 @@ double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3&
result *= x; result *= x;
} }
hadMeas[nucIndex] = true; // hadMeas[nucIndex] = true; TODO
} }
} }
else else

62
code/Settings.h
View File

@@ -16,10 +16,17 @@ namespace Settings {
/** describes one dataset (map, training, parameter-estimation, ...) */ /** describes one dataset (map, training, parameter-estimation, ...) */
// NUC Gen6 - Intel AC 8260
const MACAddress NUC1("38:de:ad:6d:77:25"); const MACAddress NUC1("38:de:ad:6d:77:25");
const MACAddress NUC2("38:de:ad:6d:60:ff"); const MACAddress NUC2("38:de:ad:6d:60:ff");
const MACAddress NUC3("1c:1b:b5:ef:a2:9a"); const MACAddress NUC3("1c:1b:b5:ef:a2:9a");
const MACAddress NUC4("1c:1b:b5:ec:d1:82"); const MACAddress NUC4("1c:1b:b5:ec:d1:82");
// NUC Gen7 - Intel AC 9461
const MACAddress NUC5("d0:c6:37:bc:5c:41");
const MACAddress NUC6("d0:c6:37:bc:77:8a");
const MACAddress NUC7("d0:c6:37:bc:77:ad");
const MACAddress NUC8("d0:c6:37:bc:6b:4b");
static int nucIndex(const MACAddress& addr) static int nucIndex(const MACAddress& addr)
{ {
@@ -27,6 +34,10 @@ namespace Settings {
if (addr == Settings::NUC2) return 1; if (addr == Settings::NUC2) return 1;
if (addr == Settings::NUC3) return 2; if (addr == Settings::NUC3) return 2;
if (addr == Settings::NUC4) return 3; if (addr == Settings::NUC4) return 3;
if (addr == Settings::NUC5) return 4;
if (addr == Settings::NUC6) return 5;
if (addr == Settings::NUC7) return 6;
if (addr == Settings::NUC8) return 7;
else { else {
assert(false); return 0; assert(false); return 0;
} }
@@ -40,6 +51,10 @@ namespace Settings {
case 1: return NUC2; case 1: return NUC2;
case 2: return NUC3; case 2: return NUC3;
case 3: return NUC4; case 3: return NUC4;
case 4: return NUC5;
case 5: return NUC6;
case 6: return NUC7;
case 7: return NUC8;
default: assert(false); return NUC1; default: assert(false); return NUC1;
} }
} }
@@ -285,8 +300,53 @@ namespace Settings {
{ 200, 201, 203, 104, 204, 205, 206, 207, 206, 208, 209, 210, 211, 212 }, { 200, 201, 203, 104, 204, 205, 206, 207, 206, 208, 209, 210, 211, 212 },
true true
}; };
// 10 Path: SHL Path 2; NUCs steht in den Räumen; Neue gen7 NUCs mit interner Antenne
const DataSetup Path10 = {
"path10",
mapDir + "shl_nuc_2.xml",
{
dataDir + "Pixel2/path10/1470971060974.csv",
dataDir + "Pixel2/path10/1578633767584.csv",
dataDir + "Pixel3/path10/3600316210200.csv",
dataDir + "Pixel3/path10/3715912582913.csv",
},
{
// NUC, ID Pos X Y Z offset loss kalman stddev
{ NUC1, {1, { 7, 6, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 1 I.2.14
{ NUC5, {5, { 54, 46, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 5 I.2.37
{ NUC6, {6, { 46, 37, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 6 I.2.03
{ NUC7, {7, { 27, 45, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 7 I.2.28
{ NUC8, {8, { 16, 36, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 8 I.2.10
},
{ 100, 101, 102, 103, 104, 103, 102, 101, 100 },
true
};
// 11 Path: SHL Path 2; NUCs steht in den Räumen; Neue gen7 NUCs mit interner Antenne
const DataSetup Path11 = {
"path11",
mapDir + "shl_nuc_2.xml",
{
dataDir + "Pixel2/path11/1688129692555.csv",
dataDir + "Pixel2/path11/1841800940572.csv",
dataDir + "Pixel3/path11/3833358280134.csv",
dataDir + "Pixel3/path11/3997838116311.csv",
},
{
// NUC, ID Pos X Y Z offset loss kalman stddev
{ NUC1, {1, { 7, 6, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 1 I.2.14
{ NUC5, {5, { 54, 46, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 5 I.2.37
{ NUC6, {6, { 46, 37, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 6 I.2.03
{ NUC7, {7, { 27, 45, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 7 I.2.28
{ NUC8, {8, { 16, 36, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 8 I.2.10
},
{ 100, 101, 102, 103, 104, 105, 104, 103, 102, 101, 100 },
true
};
} data; } data;
static DataSetup CurrentPath = data.Path9; static DataSetup CurrentPath = data.Path10;
} }

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