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
.gitignore vendored
View File

@@ -1,2 +1,3 @@
build/ build/
measurements/error measurements/error
output/

3
code/Settings.h
View File

@@ -13,6 +13,7 @@ namespace Settings {
const std::string outputDir = "../output/"; const std::string outputDir = "../output/";
const bool UseKalman = false; const bool UseKalman = false;
const bool UseRSSI = false;
/** describes one dataset (map, training, parameter-estimation, ...) */ /** describes one dataset (map, training, parameter-estimation, ...) */
@@ -465,6 +466,6 @@ namespace Settings {
}; };
} data; } 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; 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) if (assignProps)
p.weight = prob; 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 1: color = K::GnuplotColor::fromRGB(0, 255, 0); break;
case 2: color = K::GnuplotColor::fromRGB(0, 0, 255); break; case 2: color = K::GnuplotColor::fromRGB(0, 0, 255); break;
case 3: color = K::GnuplotColor::fromRGB(255, 255, 0); 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; default: color = K::GnuplotColor::fromRGB(255, 0, 0); break;
} }
float plotDist = wifi2.getFtmDist();
//float rssiDist = LogDistanceModel::rssiToDistance(-40, 2.5, wifi2.getRSSI()); if (Settings::UseRSSI)
//plot.addDistanceCircle(apPos.xy(), rssiDist, color); {
float pathLoss = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).rssi_pathloss;
plot.addDistanceCircle(apPos.xy(), wifi2.getFtmDist(), color); 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.showParticles(pf.getParticles());
plot.setCurEst(estPos); plot.setCurEst(estPos);
plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1));
plot.setTitle(Settings::UseRSSI ? "RSSI" : "FTM");
plot.addEstimationNode(estPos); plot.addEstimationNode(estPos);
//plot.setActivity((int)act.get()); //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.splot.getView().setEqualXY(true);
plot.plot(); plot.plot();
std::this_thread::sleep_for(100ms); //std::this_thread::sleep_for(100ms);
} }
} }
//std::cin.get();
printErrorStats(errorStats); printErrorStats(errorStats);
std::ofstream plot_out; std::ofstream plot_out;
@@ -752,8 +762,6 @@ int main(int argc, char** argv)
//std::vector<std::array<float, 3>> error; //std::vector<std::array<float, 3>> error;
//std::ofstream error_out; //std::ofstream error_out;
//error_out.open(Settings::errorDir + evaluationName + "_error_path1" + ".csv", std::ios_base::app); //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)) for (auto it = density.begin(); ; std::advance(it, 2))
{ {
if (it > density.end()) if (it >= density.end())
break; break;
auto p = *it; auto p = *it;
@@ -179,28 +179,15 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
plot.plot(); plot.plot();
// wifi // 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; std::vector<WiFiMeasurement> obs;
Timestamp lastTimestamp = Timestamp::fromMS(0); Timestamp lastTimestamp = Timestamp::fromMS(0);
const float sigma = 3.5; //const float sigma = 3.5;
const int movAvgWnd = 15; //const int movAvgWnd = 15;
std::array<MovingAVG<float>, 4> movAvgsFtm{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} }; //std::array<MovingAVG<float>, 4> movAvgsFtm{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
std::array<MovingAVG<float>, 4> movAvgsRssi{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} }; //std::array<MovingAVG<float>, 4> movAvgsRssi{ {movAvgWnd,movAvgWnd,movAvgWnd,movAvgWnd} };
std::vector<float> errorValuesFtm, errorValuesRssi; std::vector<float> errorValuesFtm, errorValuesRssi;
std::vector<int> timestamps; 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) void printErrorStats(std::ostream& stream, const CombinedStats<T>& errorStats)
{ {
stream << "Walk error:" << "\n"; 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 << "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() << "\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; stream << std::endl;
} }