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

Reworked trilat code

Browse Source
This commit is contained in:
Markus Bullmann
2019-11-27 16:59:03 +01:00
parent 992b8edc60
commit 3433bdaf66
5 changed files with 394 additions and 238 deletions
Download Patch File Download Diff File Expand all files Collapse all files

330
code/mainTrilat.cpp
View File

@@ -35,20 +35,29 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
{
// reading file
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
Offline::FileReader fr(setup.training[walkIdx]);
Offline::FileReader fr(setup.training[walkIdx], setup.HasNanoSecondTimestamps);
// ground truth
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, setup.gtPath);
std::vector<int> gtPath = setup.gtPath;
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
CombinedStats<float> errorStats;
//calculate distance of path
std::vector<Interpolator<uint64_t, Point3>::InterpolatorEntry> gtEntries = gtInterpolator.getEntries();
double distance = 0;
double gtTotalDistance = 0;
Stats::Statistics<double> gtWalkingSpeed;
for (int i = 1; i < gtEntries.size(); ++i) {
distance += gtEntries[i].value.getDistance(gtEntries[i - 1].value);
double distance = gtEntries[i].value.getDistance(gtEntries[i - 1].value);
double timeDiff = static_cast<double>(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;
// debug show
//MeshPlotter dbg;
@@ -60,138 +69,139 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
Plotty plot(map);
plot.buildFloorplan();
plot.setGroundTruth(setup.gtPath);
plot.setGroundTruth(gtPath);
plot.setView(30, 0);
// APs Positions
for (auto& nucConfig : setup.NUCs)
{
plot.addCircle(10000 + nucConfig.second.ID, nucConfig.second.position.xy(), 0.1);
}
plot.plot();
std::vector<WiFiMeasurement> obs;
Timestamp lastTimestamp = Timestamp::fromMS(0);
Plotta::Plotta plotta("test", "C:\\Temp\\Plotta\\dataTrilat.py");
//plotta.add("apPos", apPositions);
std::vector<Point2> 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(),
};
plotta.add("apPos", apPositions);
std::vector<WifiMeas> data = filterOfflineData(fr);
const bool UseFTM = false;
const int movAvgWnd = 10;
std::array<MovingAVG<float>, 4> movAvgsFtm { {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
std::array<MovingAVG<float>, 4> movAvgsRssi { {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
std::unordered_map<MACAddress, MovingAVG<float>> movAvgsFtm;
std::unordered_map<MACAddress, MovingAVG<float>> movAvgsRssi;
for (auto& nucConfig : setup.NUCs)
{
movAvgsFtm.insert({ nucConfig.first, MovingAVG<float>(movAvgWnd) });
movAvgsRssi.insert({ nucConfig.first, MovingAVG<float>(movAvgWnd) });
}
std::vector<float> errorValuesFtm, errorValuesRssi;
std::vector<int> timestamps;
std::vector<Point2> gtPath, estPathFtm, estPathRssi;
std::vector<Point2> estPathFtm, estPathRssi;
for (const WifiMeas& wifi : data)
for (const Offline::Entry& e : fr.getEntries())
{
Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(wifi.ts.ms())).xy();
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
gtPath.push_back(gtPos);
float distErrorFtm = 0;
float distErrorRssi = 0;
//if (wifi.numSucessMeas() == 4)
{
// FTM
{
std::vector<float> avgDists;
for (size_t i = 0; i < 4; i++)
{
float dist = wifi.ftmDists[i];
if (!isnan(dist))
{
movAvgsFtm[i].add(dist);
}
if (movAvgsFtm[i].getNumUsed() == 0)
{
avgDists.push_back(0);
}
else
{
avgDists.push_back(movAvgsFtm[i].get());
}
}
Point2 estPos = Trilateration::calculateLocation2d(apPositions, avgDists);
plot.setCurEst(Point3(estPos.x, estPos.y, 0.1));
plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1));
// draw wifi ranges
for (size_t i = 0; i < 4; i++)
{
plot.addCircle(i + 1, apPositions[i], avgDists[i]);
}
// Error
distErrorFtm = gtPos.getDistance(estPos);
errorStats.ftm.add(distErrorFtm);
estPathFtm.push_back(estPos);
}
// RSSI
{
std::vector<float> avgDists;
for (size_t i = 0; i < 4; i++)
{
float dist = wifi.rssiDists[i];
if (!isnan(dist))
{
movAvgsRssi[i].add(dist);
}
if (movAvgsRssi[i].getNumUsed() == 0)
{
avgDists.push_back(0);
}
else
{
avgDists.push_back(movAvgsRssi[i].get());
}
}
Point2 estPos = Trilateration::calculateLocation2d(apPositions, avgDists);
plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1));
// Error
distErrorRssi = gtPos.getDistance(estPos);
errorStats.rssi.add(distErrorRssi);
estPathRssi.push_back(estPos);
}
//std::cout << wifi.ts.ms() << " " << distError << "\n";
errorValuesFtm.push_back(distErrorFtm);
errorValuesRssi.push_back(distErrorRssi);
timestamps.push_back(wifi.ts.ms());
plotta.add("t", timestamps);
plotta.add("errorFtm", errorValuesFtm);
plotta.add("errorRssi", errorValuesRssi);
plotta.frame();
if (e.type != Offline::Sensor::WIFI_FTM) {
continue;
}
// TIME
const Timestamp ts = Timestamp::fromMS(e.ts);
auto wifiFtm = fr.getWifiFtm()[e.idx].data;
obs.push_back(wifiFtm);
if (ts - lastTimestamp >= Timestamp::fromMS(500))
{
// Do update
Point2 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())).xy();
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
std::unordered_map<MACAddress, std::pair<float, float>> apPosDistMap;
for (const WiFiMeasurement& wifi : obs)
{
if (wifi.getNumSuccessfulMeasurements() < 3)
continue;
const MACAddress& mac = wifi.getAP().getMAC();
float ftm_offset = setup.NUCs.at(mac).ftm_offset;
float ftmDist = wifi.getFtmDist() + ftm_offset;
float rssi_pathloss = setup.NUCs.at(mac).rssi_pathloss;
float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI());
movAvgsFtm[mac].add(ftmDist);
movAvgsRssi[mac].add(rssiDist);
apPosDistMap[mac] = { movAvgsFtm[mac].get(), movAvgsRssi[mac].get() };
}
if (apPosDistMap.size() > 3)
{
// Do update for real
std::vector<Point2> apPositions;
std::vector<float> ftmDists;
std::vector<float> rssiDists;
for (const auto& kvp : apPosDistMap)
{
apPositions.push_back(setup.NUCs.at(kvp.first).position.xy());
ftmDists.push_back(kvp.second.first);
rssiDists.push_back(kvp.second.second);
}
Point2 estFtmPos = Trilateration::levenbergMarquardt(apPositions, ftmDists);
Point2 estRssiPos = Trilateration::levenbergMarquardt(apPositions, rssiDists);
// Error
float distErrorFtm = gtPos.getDistance(estFtmPos);
errorStats.ftm.add(distErrorFtm);
estPathFtm.push_back(estFtmPos);
float distErrorRssi = gtPos.getDistance(estRssiPos);
errorStats.rssi.add(distErrorRssi);
estPathRssi.push_back(estRssiPos);
errorValuesFtm.push_back(distErrorFtm);
errorValuesRssi.push_back(distErrorRssi);
timestamps.push_back(ts.ms());
plotta.add("t", timestamps);
plotta.add("errorFtm", errorValuesFtm);
plotta.add("errorRssi", errorValuesRssi);
plotta.frame();
// Plot
plot.setCurEst(Point3(estFtmPos.x, estFtmPos.y, 0.1));
plot.addEstimationNode(Point3(estFtmPos.x, estFtmPos.y, 0.1));
plot.addEstimationNode2(Point3(estRssiPos.x, estRssiPos.y, 0.1));
// draw wifi ranges
if (Settings::PlotCircles)
{
plot.clearDistanceCircles();
for (size_t i = 0; i < ftmDists.size(); i++)
{
plot.addDistanceCircle(apPositions[i], ftmDists[i], K::GnuplotColor::fromRGB(255, 0, 0));
plot.addDistanceCircle(apPositions[i], rssiDists[i], K::GnuplotColor::fromRGB(0, 255, 0));
}
}
plot.plot();
Sleep(100);
}
obs.clear();
lastTimestamp = ts;
}
plot.plot();
//Sleep(250);
printf("");
}
plotta.add("gtPath", gtPath);
plotta.add("estPathFtm", estPathFtm);
plotta.add("estPathRssi", estPathRssi);
plotta.frame();
@@ -211,46 +221,64 @@ int mainTrilat(int argc, char** argv)
CombinedStats<float> statsQuantil;
CombinedStats<float> tmp;
std::string evaluationName = "prologic/tmp";
std::string evaluationName = "prologic/trilat";
for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++)
std::vector<Settings::DataSetup> setupsToRun = {
//Settings::data.Path5,
//Settings::data.Path7,
//Settings::data.Path8,
//Settings::data.Path9,
//Settings::data.Path10,
//Settings::data.Path11
//Settings::data.Path20,
Settings::data.Path21,
//Settings::data.Path22,
};
for (Settings::DataSetup setupToRun : setupsToRun)
{
std::cout << "Executing walk " << walkIdx << "\n";
for (int i = 0; i < 1; ++i)
Settings::CurrentPath = setupToRun;
for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++)
{
std::cout << "Start of iteration " << i << "\n";
std::cout << "Executing walk " << walkIdx << "\n";
for (int i = 0; i < 1; ++i)
{
std::cout << "Start of iteration " << i << "\n";
tmp = run(Settings::CurrentPath, walkIdx, evaluationName);
tmp = run(Settings::CurrentPath, walkIdx, evaluationName);
statsAVG.ftm.add(tmp.ftm.getAvg());
statsMedian.ftm.add(tmp.ftm.getMedian());
statsSTD.ftm.add(tmp.ftm.getStdDev());
statsQuantil.ftm.add(tmp.ftm.getQuantile(0.75));
statsAVG.ftm.add(tmp.ftm.getAvg());
statsMedian.ftm.add(tmp.ftm.getMedian());
statsSTD.ftm.add(tmp.ftm.getStdDev());
statsQuantil.ftm.add(tmp.ftm.getQuantile(0.75));
statsAVG.rssi.add(tmp.rssi.getAvg());
statsMedian.rssi.add(tmp.rssi.getMedian());
statsSTD.rssi.add(tmp.rssi.getStdDev());
statsQuantil.rssi.add(tmp.rssi.getQuantile(0.75));
statsAVG.rssi.add(tmp.rssi.getAvg());
statsMedian.rssi.add(tmp.rssi.getMedian());
statsSTD.rssi.add(tmp.rssi.getStdDev());
statsQuantil.rssi.add(tmp.rssi.getQuantile(0.75));
std::cout << "Iteration " << i << " completed" << std::endl;
std::cout << "Iteration " << i << " completed" << std::endl;
}
}
std::cout << "Results for path " << Settings::CurrentPath.name << std::endl;
std::cout << "==========================================================" << std::endl;
std::cout << "Average of all statistical data FTM: " << std::endl;
std::cout << "Median: " << statsMedian.ftm.getAvg() << std::endl;
std::cout << "Average: " << statsAVG.ftm.getAvg() << std::endl;
std::cout << "Standard Deviation: " << statsSTD.ftm.getAvg() << std::endl;
std::cout << "75 Quantil: " << statsQuantil.ftm.getAvg() << std::endl;
std::cout << "==========================================================" << std::endl;
std::cout << "==========================================================" << std::endl;
std::cout << "Average of all statistical data RSSI: " << std::endl;
std::cout << "Median: " << statsMedian.rssi.getAvg() << std::endl;
std::cout << "Average: " << statsAVG.rssi.getAvg() << std::endl;
std::cout << "Standard Deviation: " << statsSTD.rssi.getAvg() << std::endl;
std::cout << "75 Quantil: " << statsQuantil.rssi.getAvg() << std::endl;
std::cout << "==========================================================" << std::endl;
}
std::cout << "==========================================================" << std::endl;
std::cout << "Average of all statistical data FTM: " << std::endl;
std::cout << "Median: " << statsMedian.ftm.getAvg() << std::endl;
std::cout << "Average: " << statsAVG.ftm.getAvg() << std::endl;
std::cout << "Standard Deviation: " << statsSTD.ftm.getAvg() << std::endl;
std::cout << "75 Quantil: " << statsQuantil.ftm.getAvg() << std::endl;
std::cout << "==========================================================" << std::endl;
std::cout << "==========================================================" << std::endl;
std::cout << "Average of all statistical data RSSI: " << std::endl;
std::cout << "Median: " << statsMedian.rssi.getAvg() << std::endl;
std::cout << "Average: " << statsAVG.rssi.getAvg() << std::endl;
std::cout << "Standard Deviation: " << statsSTD.rssi.getAvg() << std::endl;
std::cout << "75 Quantil: " << statsQuantil.rssi.getAvg() << std::endl;
std::cout << "==========================================================" << std::endl;
return 0;
}