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

Changes

Browse Source
This commit is contained in:
Markus Bullmann
2019-11-13 15:12:44 +01:00
parent d645ab7675
commit ac824937aa
3 changed files with 188 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
code/Settings.h
View File

@@ -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;
}

169
code/main.cpp
View File

@@ -232,7 +232,18 @@ void exportFtmValues(Offline::FileReader& fr, Interpolator<uint64_t, Point3>& gt
fs.close();
}
template<typename T>
struct TimeSeries
{
std::vector<Timestamp> t;
std::vector<T> 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<float> run(Settings::DataSetup setup, int walkIdx, std::str
//calculate distance of path
std::vector<Interpolator<uint64_t, Point3>::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<double> gtWalkingSpeed;
for (int i = 1; i < gtEntries.size(); ++i) {
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;
// error file
const long int t = static_cast<long int>(time(NULL));
@@ -345,7 +364,12 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
std::vector<float> errorValuesFtm, errorValuesRssi;
std::vector<int> timestamps;
std::vector<std::array<float, 4>> gtDistances, ftmDistances, rssiDistances; // distance per AP
std::vector<int> timestampsDist;
std::vector<std::array<float, 4>> gtDistances, rssiDistances; // distance per AP
std::array<TimeSeries<std::array<float, 3>>, 4> ftmDistances;
TimeSeries<std::array<KalmanPrediction, 4>> ftmPredictions;
Plotta::Plotta errorPlot("errorPlot", outputDir.string() + "/errorData.py");
Plotta::Plotta distsPlot("distsPlot", outputDir.string() + "/distances.py");
@@ -425,6 +449,47 @@ static CombinedStats<float> 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<uint64_t>(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<KalmanPrediction, 4> 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<float> 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<float> 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<float> 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<Settings::DataSetup> setupsToRun = { Settings::data.Path7,
Settings::data.Path8,
Settings::data.Path9 };
std::vector<Settings::DataSetup> setupsToRun = {
//Settings::data.Path5,
Settings::data.Path7,
Settings::data.Path8,
//Settings::data.Path9
};
for (Settings::DataSetup setupToRun : setupsToRun)
{

24
code/mainProb.cpp
View File

@@ -237,7 +237,7 @@ static CombinedStats<float> 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<float> 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<float> 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);