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

current code and TeX. code fine?!?!?!

Browse Source
This commit is contained in:
FrankE
2017-04-29 20:57:12 +02:00
parent fc72a75f57
commit 60712689cf
41 changed files with 804 additions and 234 deletions
Download Patch File Download Diff File Expand all files Collapse all files

398
pf/EvalWalk.h Normal file → Executable file
View File

@@ -44,6 +44,15 @@
#include <Indoor/sensors/offline/FileReader.h>
#include <Indoor/sensors/offline/Listener.h>
#define PLOT_LIVE
//#define PLOT_WIFI
//#define PLOT_ERROR_TIME
//#define PLOT_ERROR_FUNC
#define PLOT_UPDATE_STEP 20
class EvalWalk : public Offline::Listener {
Grid<MyGridNode>* grid;
@@ -53,6 +62,8 @@ class EvalWalk : public Offline::Listener {
Plotty plotty;
PlotWifiMeasurements plotWifi;
PlotErrTime pet;
PlotErrFunc pef;
Offline::FileReader reader;
Offline::FilePlayer player;
@@ -74,11 +85,15 @@ class EvalWalk : public Offline::Listener {
EarthMapping em;
std::string walkName;
float absHead = 0;
public:
EvalWalk(Floorplan::IndoorMap* map) : plotty(map), map(map), em(map) {
EvalWalk(Floorplan::IndoorMap* map) : plotty(map), map(map), em(map),
pet("\\small{time (sec)}", "\\small{error (m)}", "\\small{APs visible}"),
pef("\\small{error (m)}", "\\small{updates (\\%)}") {
const std::string saveFile = Settings::pathData + "/grid.dat";
grid = new Grid<MyGridNode>(Settings::Grid::gridSize_cm);
@@ -108,8 +123,8 @@ public:
pf->setTransition( std::unique_ptr<PFTrans>( new PFTrans(grid)) );
// resampling step?
//pf->setNEffThreshold(0.15);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
pf->setNEffThreshold(0.02);
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
//pf->setNEffThreshold(0.75);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.10)) );
@@ -117,9 +132,9 @@ public:
//pf->setNEffThreshold(0.75);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.05)) );
K::ParticleFilterResamplingNEff<MyState>* res = new K::ParticleFilterResamplingNEff<MyState>(0.75, 0.05);
pf->setNEffThreshold(1.0);
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingNEff<MyState>>(res) );
// K::ParticleFilterResamplingNEff<MyState>* res = new K::ParticleFilterResamplingNEff<MyState>(0.75, 0.05);
// pf->setNEffThreshold(1.0);
// pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingNEff<MyState>>(res) );
// move during resampling. NOT ALLOWED!
// res->setDrawCallback([&] (K::Particle<MyState>& p) {
@@ -145,6 +160,211 @@ public:
}
struct LeWalk {
std::string name;
float absHead;
std::string walkFile;
std::vector<int> gtIndices;
LeWalk(const std::string& name, float absHead, std::string walkFile, std::vector<int> gtIndices) :
name(name), absHead(absHead), walkFile(walkFile), gtIndices(gtIndices) {
;
}
};
struct LeModel {
std::string name;
WiFiModel* model;
LeModel(const std::string& name, WiFiModel* model) : name(name), model(model) {;}
};
static void serialize(const K::Statistics<float>& stats, const LeWalk& walk, const LeModel& model, const int idx) {
const std::string file = Settings::fPathGFX + "/walks/data/" + walk.name + "_" + model.name + "_" + std::to_string(idx) + ".txt";
std::ofstream out(file);
for (const float f : stats.getAll()) {
out << f << "\n";
}
out.close();
}
static K::Statistics<float>* deserialize(const LeWalk& walk, const LeModel& model, const int idx) {
const std::string file = Settings::fPathGFX + "/walks/data/" + walk.name + "_" + model.name + "_" + std::to_string(idx) + ".txt";
K::Statistics<float>* res = new K::Statistics<float>();
std::ifstream inp(file);
while(inp) {
float f;
inp >> f;
res->add(f);
}
return res;
}
static std::vector<LeWalk> allWalks() {
return {
LeWalk("path1a", M_PI/2, Settings::path1a, Settings::GroundTruth::path1),
LeWalk("path1b", M_PI/2, Settings::path1b, Settings::GroundTruth::path1),
LeWalk("toni-all-1a", M_PI/2, Settings::path_toni_all_1a, Settings::GroundTruth::path1),
LeWalk("toni-all-1b", M_PI/2, Settings::path_toni_all_1b, Settings::GroundTruth::path1),
LeWalk("path2a", 0, Settings::path2a, Settings::GroundTruth::path2),
LeWalk("path2b", 0, Settings::path2b, Settings::GroundTruth::path2),
LeWalk("toni-all-2a", M_PI/2, Settings::path_toni_all_2a, Settings::GroundTruth::path2),
LeWalk("toni-all-2b", M_PI/2, Settings::path_toni_all_2b, Settings::GroundTruth::path2),
/////////LeWalk("toni-inst-1a", M_PI/2, Settings::path_toni_inst_1a, Settings::GroundTruth::path_toni_inst_1),
LeWalk("toni-inst-1b", M_PI/2, Settings::path_toni_inst_1b, Settings::GroundTruth::path_toni_inst_1),
LeWalk("toni-inst-2a", M_PI/2, Settings::path_toni_inst_2a, Settings::GroundTruth::path_toni_inst_2),
LeWalk("toni-inst-2b", M_PI/2, Settings::path_toni_inst_2b, Settings::GroundTruth::path_toni_inst_2),
LeWalk("toni-inst-3a", M_PI/2, Settings::path_toni_inst_3a, Settings::GroundTruth::path_toni_inst_3),
LeWalk("toni-inst-3b", M_PI/2, Settings::path_toni_inst_3b, Settings::GroundTruth::path_toni_inst_3),
};
}
static std::vector<LeModel> allModels(Floorplan::IndoorMap* map) {
WiFiModelFactory fac(map);
return {
//LeModel("empiric", fac.loadXML(Settings::wifiAllFixed)),
//LeModel("opt 1", fac.loadXML(Settings::wifiAllOptPar)),
//LeModel("opt 2", fac.loadXML(Settings::wifiEachOptPar)),
//LeModel("opt 3", fac.loadXML(Settings::wifiEachOptParPos)),
//LeModel("per floor", fac.loadXML(Settings::wifiEachOptParPos_multimodel)),
LeModel("per bbox", fac.loadXML(Settings::wifiEachOptParPos_perBBox)),
};
}
static constexpr int numRepeats = 1;
/** create GFX for all previously walked parts */
static void walkEverythingBuildStats(Floorplan::IndoorMap* map) {
const std::vector<LeWalk> walks = allWalks();
const std::vector<LeModel> models = allModels(map);
PlotErrFunc* pefAll = new PlotErrFunc();
int numStuck = 0;
int numTotal = 0;
for (const LeModel& mdl : models) {
K::Statistics<float>* modelStatsAvg = new K::Statistics<float>();
K::Statistics<float>* modelStatsMed = new K::Statistics<float>();
K::Statistics<float>* modelStatsSingle = new K::Statistics<float>();
pefAll->add(mdl.name, modelStatsSingle);
// ... to perform every walk...
for (const LeWalk& walk : walks) {
for (int i = 0; i < numRepeats; ++i) {
++numTotal;
K::Statistics<float>* walkStats = deserialize(walk, mdl, i);
if (walkStats->getMedian() < 10) {
modelStatsSingle->add(*walkStats);
pefAll->plot();
} else {
++numStuck;
}
}
}
sleep(1);
}
std::cout << "stuck: " << numStuck << " (" << (numStuck*100/numTotal) << "%)" << std::endl;
sleep(100);
}
/** perform all walks and write them to file */
static void walkEverything(Floorplan::IndoorMap* map) {
const std::vector<LeWalk> walks = allWalks();
const std::vector<LeModel> models = allModels(map);
PlotErrFunc* pefAll = new PlotErrFunc();
// use every model ...
for (const LeModel& mdl : models) {
K::Statistics<float>* modelStatsSingle = new K::Statistics<float>();
pefAll->add(mdl.name, modelStatsSingle);
// ... to perform every walk...
for (const LeWalk& walk : walks) {
//K::Statistics<float> curWalkRepeatStatsAvg;
//K::Statistics<float> curWalkRepeatStatsMed;
// ... several times
for (int i = 0; i < numRepeats; ++i) {
EvalWalk ew(map);
ew.walkName = walk.name + " - " + mdl.name + " - " + std::to_string(i);
// get ground-truth
ew.absHead = walk.absHead;
ew.groundTruth = FloorplanHelper::getGroundTruth(map, walk.gtIndices);
// eval
std::unique_ptr<PFEval> eval = std::unique_ptr<PFEval>( new PFEval(ew.grid, *mdl.model, ew.em) );
ew.pf->setEvaluation( std::move(eval) );
// data-file
ew.reader.open(walk.walkFile);
ew.groundTruthLive = ew.reader.getGroundTruth(map, walk.gtIndices);
ew.player.setReader(&ew.reader);
ew.player.setListener(&ew);
ew.player.start();
// wait for completion
ew.player.join();
// write plots
ew.writeToFile();
// add every single error for each timestamp to the overall model error
modelStatsSingle->reset();
modelStatsSingle->add(ew.statsErr);
// export this walks's statistics
serialize(ew.statsErr, walk, mdl, i);
pefAll->plot();
}
// all repeats done
const std::string walkname = walk.name + " - " + mdl.name;
std::cout << "walk result for " << walkname << std::endl;
//std::cout << "\t" << curWalkRepeatStatsAvg.asString() << std::endl;
//std::cout << "\t" << curWalkRepeatStatsMed.asString() << std::endl;
std::cout << std::endl;
//sleep(1);
}
// all walks for one model done
std::cout << "MODEL RESULTS for " << mdl.name << std::endl;
//std::cout << "\t" << modelStatsAvg->asString() << std::endl;
//std::cout << "\t" << modelStatsMed->asString() << std::endl;
std::cout << std::endl;
std::cout << std::endl;
std::cout << std::endl;
sleep(2);
}
}
void walk1() {
// path1
@@ -211,7 +431,7 @@ public:
++curCtrl.numStepsSinceLastTransition;
}
gotSensorData(ts);
curCtrl.activityNew = actDetect.add(ts, data);
actDetect.add(ts, data);
}
virtual void onGravity(const Timestamp ts, const GravityData data) override {
@@ -219,14 +439,18 @@ public:
}
virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) override {
std::cout << "WIFI" << std::endl;
curObs.wifi = data;
//curObs.wifi = WiFiMeasurements::mix(curObs.wifi, data);
#ifdef PLOT_WIFI
plotWifi.add(Settings::WiFiModel::vg_eval.group(data));
plotWifi.plot();
#endif
}
virtual void onBarometer(const Timestamp ts, const BarometerData data) override {
;
actDetect.add(ts, data);
curCtrl.activityNew = actDetect.get();
curObs.activityNew = actDetect.get();
}
virtual void onGPS(const Timestamp ts, const GPSData data) override {
@@ -300,12 +524,48 @@ private:
return set.size();
}
void writeToFile() {
std::string path = Settings::fPathGFX;
std::string base = path + "/walks/" + "walk-" + walkName;
plotty.gp << "unset arrow 1\n";
plotty.gp << "unset arrow 2\n";
plotty.gp << "unset colorbox\n";
plotty.gp << "unset border\n";
plotty.gp << "set view equal xy\n";
plotty.splot.getView().setCamera(74,30);
plotty.splot.getView().setScaleAll(3.5);
plotty.splot.getAxisX().setTicsVisible(false);
plotty.splot.getAxisY().setTicsVisible(false);
plotty.splot.getAxisZ().setTicsVisible(false);
plotty.particles.clear();
plotty.gp.setTerminal("epslatex", K::GnuplotSize(8.6, 4.5));
plotty.gp.setOutput(base + "-map.tex");
plotty.writeCodeTo(base + "-map.gp");
plotty.plot();
pef.getPlot().getMargin().set(4, 0.2, 0.1, 2.0);
pef.getGP().setTerminal("epslatex", K::GnuplotSize(8.6, 3.0));
pef.getGP().setOutput(base + "-error-cum.tex");
pef.writePlotToFile(base + "-error-cum.gp");
pef.plot();
pet.getPlot().getMargin().set(4, 0.2, 0.1, 2.0);
pet.getGP().setTerminal("epslatex", K::GnuplotSize(8.6, 3.0));
pet.getGP().setOutput(base + "-error-time.tex");
pet.writePlotToFile(base + "-error-time.gp");
pet.plot();
}
/** perform a filter-update (called from a background-loop) */
void filterUpdate() {
++updateCount;
static PlotErrTime pet("\\small{time (sec)}", "\\small{error (m)}", "\\small{APs visible}");
static PlotErrFunc pef("\\small{error (m)}", "\\small{updates (\\%)}");
pef.showMarkers(true, true);
std::cout << "update" << std::endl;
@@ -319,22 +579,23 @@ private:
const Point3 curGT = groundTruthLive.get(lastTransition);
plotty.setCurEst(curEst.position.inMeter());
plotty.setGroundTruth(curGT);
if (updateCount > 4) {
// start the error-over-time plot after some filter updates
if (updateCount > 12) {
// error between ground-truth and estimation
const float estRealErr = curEst.position.inMeter().getDistance(curGT);
statsErr.add(estRealErr);
#ifdef PLOT_ERROR_FUNC
pef.clear();
pef.add("", &statsErr);
pef.plot();
#endif
#ifdef PLOT_ERROR_TIME
// timed error
pet.addErr(lastTransition, estRealErr);
pet.addB(lastTransition, getNumFHWSAPs(curObs.wifi));
pet.plot();
#endif
// update estimated path
const K::GnuplotPoint3 p3(curEst.position.x_cm, curEst.position.y_cm, curEst.position.z_cm);
@@ -342,50 +603,73 @@ private:
}
std::cout << statsErr.asString() << std::endl;
// show particles
float maxWeight = 0;
float minWeight = 99;
plotty.particles.clear();
for (const auto p : pf->getParticles()) {
const K::GnuplotPoint3 p3(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
plotty.particles.add(p3/100, p.weight);
if (p.weight > maxWeight) {maxWeight = p.weight;}
if (p.weight < minWeight) {minWeight = p.weight;}
}
plotty.gp << "set cbrange [" << minWeight << ":" << maxWeight << "] \n";
// show ground-truth
plotty.pathReal.clear();
for (const Point3 pt : groundTruth) {
plotty.pathReal.add(K::GnuplotPoint3(pt.x, pt.y, pt.z));
}
std::string title =
" time " + std::to_string(curObs.currentTime.sec()) +
" steps: " + std::to_string(ctrlCopy.numStepsSinceLastTransition) +
" turn: " + std::to_string(ctrlCopy.turnSinceLastTransition_rad) +
" APs: " + std::to_string(curObs.wifi.entries.size()) +
" Act: " + std::to_string((int)curCtrl.activityNew);
plotty.setTitle(title);
// relative heading and compass
{
Point2 cen(0.1, 0.9);
Point2 dir(std::cos(absHead), std::sin(absHead));
Point2 arr = cen + dir * 0.1;
plotty.gp << "set arrow 1 from screen " << cen.x << "," << cen.y << " to screen " << arr.x << "," << arr.y << "\n";
dir = Point2(std::cos(ctrlCopy.compassAzimuth_rad), std::sin(ctrlCopy.compassAzimuth_rad));
arr = cen + dir * 0.05;
plotty.gp << "set arrow 2 from screen " << cen.x << "," << cen.y << " to screen " << arr.x << "," << arr.y << "\n";
}
// plot
plotty.plot();
static int cnt = 0;
if (++cnt % PLOT_UPDATE_STEP == 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(5));
std::cout << statsErr.asString() << std::endl;
// estimation and ground-truth
plotty.setCurEst(curEst.position.inMeter());
plotty.setGroundTruth(curGT);
// show particles
float maxWeight = 0;
float minWeight = 99;
plotty.particles.clear();
for (int i = 0; i < pf->getParticles().size(); i += 10) {
const auto p = pf->getParticles()[i];
const K::GnuplotPoint3 p3(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
plotty.particles.add(p3/100, p.weight);
if (p.weight > maxWeight) {maxWeight = p.weight;}
if (p.weight < minWeight) {minWeight = p.weight;}
}
plotty.gp << "set cbrange [" << minWeight << ":" << maxWeight << "] \n";
// show ground-truth
plotty.pathReal.clear();
for (const Point3 pt : groundTruth) {
plotty.pathReal.add(K::GnuplotPoint3(pt.x, pt.y, pt.z));
}
std::string title =
" time " + std::to_string(curObs.currentTime.sec()) +
" steps: " + std::to_string(ctrlCopy.numStepsSinceLastTransition) +
" turn: " + std::to_string(ctrlCopy.turnSinceLastTransition_rad) +
" APs: " + std::to_string(curObs.wifi.entries.size()) +
" Act: " + std::to_string((int)curCtrl.activityNew) +
" " + walkName;
plotty.setTitle(title);
// relative heading and compass
{
Point2 cen(0.1, 0.9);
Point2 dir(std::cos(absHead), std::sin(absHead));
Point2 arr = cen + dir * 0.1;
plotty.gp << "set arrow 1 from screen " << cen.x << "," << cen.y << " to screen " << arr.x << "," << arr.y << "\n";
dir = Point2(std::cos(ctrlCopy.compassAzimuth_rad), std::sin(ctrlCopy.compassAzimuth_rad));
arr = cen + dir * 0.05;
plotty.gp << "set arrow 2 from screen " << cen.x << "," << cen.y << " to screen " << arr.x << "," << arr.y << "\n";
}
#ifdef PLOT_LIVE
plotty.plot();
#endif
#ifdef PLOT_ERROR_TIME
pet.plot();
#endif
#ifdef PLOT_ERROR_FUNC
pef.plot();
#endif
}
//std::this_thread::sleep_for(std::chrono::milliseconds(5));
curCtrl.resetAfterTransition();

98
pf/PF.h Normal file → Executable file
View File

@@ -98,7 +98,8 @@ struct MyObservation {
// TODO: switch to a general activity enum/detector for barometer + accelerometer + ...?
/** detected activity */
ActivityButterPressure::Activity activity;
//ActivityButterPressure::Activity activity;
Activity activityNew = Activity::STANDING;
/** time of evaluation */
Timestamp currentTime;
@@ -119,9 +120,9 @@ struct MyControl {
// TODO: switch to a general activity enum/detector using barometer + accelerometer?
/** currently detected activity */
ActivityButterPressure::Activity activity = ActivityButterPressure::Activity::STAY;
//ActivityButterPressure::Activity activity = ActivityButterPressure::Activity::STAY;
Activity activityNew;
Activity activityNew = Activity::STANDING;
/** reset the control-data after each transition */
void resetAfterTransition() {
@@ -150,8 +151,10 @@ public:
std::uniform_real_distribution<float> distHead(0, 2*M_PI);
for (K::Particle<MyState>& p : particles) {
const int idx = distIdx(gen);
const MyGridNode& node = (*grid)[idx];
again:
const int idx = distIdx(gen);
const MyGridNode& node = (*grid)[idx];
if (node.getNumNeighbors() != 8) {goto again;}
p.state.position = node; // random position
p.state.heading.direction = Heading(distHead(gen)); // random heading
p.weight = 1.0 / particles.size(); // equal weight
@@ -174,7 +177,7 @@ public:
WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
//WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modRelHead;
WalkModuleAbsoluteHeadingControl<MyGridNode, MyState, MyControl> modAbsHead;
@@ -183,7 +186,11 @@ public:
public:
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modRelHead(&ctrl, Settings::IMU::turnSigma), modAbsHead(&ctrl, Settings::IMU::absHeadSigma), modDestination(*grid), modActivity(&ctrl) {
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modRelHead(&ctrl, Settings::IMU::turnSigma),
modAbsHead(&ctrl, Settings::IMU::absHeadSigma),
modDestination(*grid)
//,modActivity(&ctrl)
{
walker.addModule(&modRelHead);
//walker.addModule(&modAbsHead);
@@ -234,10 +241,9 @@ public:
K::Particle<MyState>& p = particles[i];
// first transitions: more variation as the state is unknown
if (numTrans < 50 || numTrans % 10 == 0) {
if (numTrans < 70 || numTrans % (10*1) == 0) {
const MyGridNode* n = grid->getNodePtrFor(p.state.position);
std::normal_distribution<float> distTurn(0, 0.4);
for (int j = 0; j < 5; ++j) {
std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
@@ -246,9 +252,10 @@ public:
}
p.state.position = *n;
if (numTrans < 50) {
p.state.heading.direction += distTurn(gen);
dist_m += 0.5;
if (numTrans < 70) {
std::normal_distribution<float> distTurn(0, 0.3);
p.state.heading.direction += distTurn(gen);
dist_m += 0.5;
}
}
@@ -264,6 +271,11 @@ public:
p.state = walker.getDestination(*grid, p.state, dist_m, prob);
p.weight *= prob; // grid-walk-probability
//const GridNode gn = grid->getNodeFor(p.state.position);
//const double probNode = (gn.getNumNeighbors() >= 8) ? (0.7) : (0.3);
//p.weight *= probNode;
if (p.weight != p.weight) {
throw Exception("nan");
}
@@ -320,14 +332,31 @@ public:
double res = 0;
if (angularDiff > Angle::degToRad(90)) {res = 0.05;}
else if (angularDiff > Angle::degToRad(45)) {res = 0.25;}
else {res = 0.70;}
if (angularDiff > Angle::degToRad(120)) {res = 0.30;}
else {res = 0.70;}
return res;
}
double getActivity(const MyState& s, const MyObservation& obs) {
const MyGridNode& n = grid->getNodeFor(s.position);
switch(obs.activityNew) {
case Activity::WALKING:
return (n.getType() != MyGridNode::TYPE_STAIR) ? (0.7) : (0.3);
case Activity::WALKING_DOWN:
case Activity::WALKING_UP:
return (n.getType() == MyGridNode::TYPE_STAIR) ? (0.7) : (0.3);
default:
return 1;
}
return 1.0;
}
WiFiQualityAnalyzer wqa;
double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override {
@@ -343,7 +372,9 @@ public:
wqa.add(wifiObs);
const float quality = wqa.getQuality();
float quality = wqa.getQuality();
std::cout << "wifi quality: " << quality << std::endl;
// GPS
const GPSProbability gpsProb(em);
@@ -353,7 +384,7 @@ public:
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
// use (0.9 * p) + (0.1 * (1-p)) for error cases
wiFiProbability.setUseError(false);
wiFiProbability.setUseError(true);
//#pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) {
@@ -362,16 +393,15 @@ public:
// WiFi free
double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, wifiObs);
double pWiFiMod = Distribution::Exponential<double>::getProbability(0.20, -std::log(pWiFi));
double pWiFiVeto = wiFiProbability.getVeto(p.state.position.inMeter()+person, observation.currentTime, wifiObs);
//double pWiFiMod = Distribution::Exponential<double>::getProbability(0.20, -std::log(pWiFi));
//double pWiFiMod = std::pow(pWifi, 0.2);
//double pWiFiVeto = wiFiProbability.getVeto(p.state.position.inMeter()+person, observation.currentTime, wifiObs);
const bool volatile init = observation.currentTime.sec() < 25;
//double pWiFiMod = (init) ? (std::pow(pWiFi, 0.1)) : (std::pow(pWiFi, 0.5));
double pWiFiMod = (init) ? (std::pow(pWiFi, 0.5)) : (std::pow(pWiFi, 0.9));
if (quality < 0.2) {
pWiFi = 1;
pWiFiVeto = 1;
std::cout << "disabling WiFi" << std::endl;
}
// WiFi grid
//const MyGridNode& node = grid->getNodeFor(p.state.position);
@@ -379,9 +409,18 @@ public:
//const double pWiFi = wiFiProbability.getProbability(node, observation.currentTime, wifiObs);
//const double pStair = 1;//getStairProb(p, observation.activity);
const double pGPS = gpsProb.getProbability(p.state.position.inMeter(), observation.gps);
const double pAbsHead = getAbsHead(p.state, observation);
const double prob = pWiFi * pWiFiVeto;// * pAbsHead;
double pGPS = gpsProb.getProbability(p.state.position.inMeter(), observation.gps);
double pAbsHead = getAbsHead(p.state, observation);
double pActivty = getActivity(p.state, observation);
// bad wifi? -> we have no idea where we are!
if (quality < 0.25 && !init) {
pWiFiMod = 1;
//p.weight = std::pow(p.weight, 0.5);
}
// overall evaluation
const double prob = pWiFiMod * pAbsHead * pActivty;
//GPS ERROR?!?!?! does it work without disabling wifi when gps is disabled?
@@ -392,6 +431,11 @@ public:
// TESTING
//p.weight = std::pow(p.weight, 0.5);
// // do NOT update weights
// if (quality < 0.25) {
// p.weight = 1;
// }
p.weight *= prob; // NOTE: keeps the weight returned by the transition step!
//p.weight = prob; // does NOT keep the weights returned by the transition step