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

Added global RSSI flag and extended prop code to handle 8 NUCs

Browse Source
This commit is contained in:
Markus Bullmann
2019-11-20 14:44:39 +01:00
parent 39aba04943
commit bdc82d04c7
6 changed files with 29 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
code/Settings.h
View File

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

2
code/filter.h
View File

@@ -288,7 +288,7 @@ struct MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
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;

24
code/main.cpp
View File

@@ -526,14 +526,19 @@ static CombinedStats<float> 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<float> 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<float> 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<std::array<float, 3>> error;
//std::ofstream error_out;
//error_out.open(Settings::errorDir + evaluationName + "_error_path1" + ".csv", std::ios_base::app);

23
code/mainProb.cpp
View File

@@ -119,7 +119,7 @@ static void plotDensity(Plotty& plot, std::vector<std::pair<Point2, double>>& 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<float> run(Settings::DataSetup setup, int walkIdx, std::str
plot.plot();
// wifi
std::array<Kalman, 4> 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<Point2, 4> 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<WiFiMeasurement> obs;
Timestamp lastTimestamp = Timestamp::fromMS(0);
const float sigma = 3.5;
const int movAvgWnd = 15;
std::array<MovingAVG<float>, 4> movAvgsFtm{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
std::array<MovingAVG<float>, 4> movAvgsRssi{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
//const float sigma = 3.5;
//const int movAvgWnd = 15;
//std::array<MovingAVG<float>, 4> movAvgsFtm{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
//std::array<MovingAVG<float>, 4> movAvgsRssi{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
std::vector<float> errorValuesFtm, errorValuesRssi;
std::vector<int> timestamps;

6
code/misc.h
View File

@@ -52,10 +52,10 @@ template<typename T>
void printErrorStats(std::ostream& stream, const CombinedStats<T>& 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;
}