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

refactoring, easy debugging

Browse Source
This commit is contained in:
toni
2017-03-31 18:34:38 +02:00
parent 9b21e5627d
commit a71f082a45
4 changed files with 103 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
code/filter/Logic.h
View File

@@ -14,6 +14,7 @@
#include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
#include <Indoor/grid/walk/v2/modules/WalkModulePreventVisited.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
@@ -46,6 +47,7 @@ struct PFInit : public K::ParticleFilterInitializer<MyState> {
p.state.heading.direction = (rand() % 360) / 180.0 * M_PI; // random heading
p.state.heading.error = 0;
p.state.relativePressure = 0; // start with a relative pressure of 0
p.weight = 1.0 / particles.size(); // equal weight
}
}
@@ -75,7 +77,7 @@ struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
p.state.heading.direction = headingDeg / 180.0 * M_PI; // fixed heading
p.state.heading.error = 0;
p.state.relativePressure = 0; // start with a relative pressure of 0
p.weight = 1.0 / particles.size(); // equal weight
}
}
@@ -96,6 +98,8 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
WalkModuleFavorZ<MyNode, MyState> modFavorZ;
//WalkModulePreventVisited<MyNode, MyState> modPreventVisited;
//WalkModuleActivityControl<MyNode, MyState, MyControl> modActivity;
std::minstd_rand gen;
@@ -104,7 +108,7 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
walker.addModule(&modHead);
//walker.addModule(&modHeadMises);
//walker.addModule(&modSpread); // might help in some situations! keep in mind!
//walker.addModule(&modActivity);
//walker.addModule(&modHeadUgly);
//walker.addModule(&modImportance);
//walker.addModule(&modFavorZ);
@@ -178,18 +182,45 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
return Distribution::Normal<double>::getProbability(static_cast<double>(hPa), 0.10, static_cast<double>(observation.relativePressure));
}
double getStairProb(const K::Particle<MyState>& p, const ActivityButterPressure::Activity act) {
const float kappa = 0.75;
const MyNode& gn = grid.getNodeFor(p.state.position);
switch (act) {
case ActivityButterPressure::Activity::STAY:
if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;}
if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;}
{return 1-kappa;}
case ActivityButterPressure::Activity::UP:
case ActivityButterPressure::Activity::DOWN:
if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;}
if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
{return 1-kappa;}
}
return 1.0;
}
virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObs& observation) override {
double sum = 0;
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
for (K::Particle<MyState>& p : particles) {
#pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) {
K::Particle<MyState>& p = particles[i];
// Point3 pos_m = p.state.position.inMeter();
// Point3 posOld_m = p.state.positionOld.inMeter();
const double pWifi = getWIFI(observation, wifiObs, p.state.position);
const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure);
const double pBaroPressure = getStairProb(p, observation.activity);
//const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure);
//const double pBeacon = getBEACON(observation, p.state.position);
//small checks
@@ -199,6 +230,8 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
const double prob = pBaroPressure * pWifi;
p.weight = prob;
#pragma omp atomic
sum += (prob);
}

5
code/filter/Structs.h
View File

@@ -18,6 +18,8 @@
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
#include <Indoor/grid/walk/v2/modules/WalkModulePreventVisited.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpread, public WalkStateFavorZ {
static Floorplan::IndoorMap* map;
@@ -77,6 +79,9 @@ struct MyObs {
/** wifi measurements */
WiFiMeasurements wifi;
/** detected activity */
ActivityButterPressure::Activity activity = ActivityButterPressure::Activity::STAY;
/** beacon measurements */
BeaconMeasurements beacons;