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

next commit

Browse Source
This commit is contained in:
FrankE
2017-04-05 18:45:00 +02:00
parent 6171582b40
commit b2adb16b49
18 changed files with 1602 additions and 360 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
pf/PF.h
View File

@@ -14,6 +14,8 @@
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
#include <Indoor/sensors/gps/GPSData.h>
#include <Indoor/sensors/gps/GPSProbability.h>
#include <Indoor/sensors/activity/Activity.h>
#include <Indoor/grid/walk/v2/GridWalker.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
@@ -113,7 +115,9 @@ struct MyControl {
// TODO: switch to a general activity enum/detector using barometer + accelerometer?
/** currently detected activity */
ActivityButterPressure::Activity activity;
ActivityButterPressure::Activity activity = ActivityButterPressure::Activity::STAY;
Activity activityNew;
/** reset the control-data after each transition */
void resetAfterTransition() {
@@ -180,10 +184,10 @@ public:
walker.addModule(&modRelHead);
walker.addModule(&modAbsHead);
walker.addModule(&modActivity);
//walker.addModule(&modFavorZ);
//walker.addModule(&modImportance);
//walker.addModule(&modActivity);
// if (Settings::destination != GridPoint(0,0,0)) {
// //walker.addModule(&modDestination);
@@ -215,6 +219,8 @@ public:
K::Particle<MyState>& p = particles[i];
p.weight = std::pow(p.weight, 0.8);
double prob;
p.state = walker.getDestination(*grid, p.state, dist_m, prob);
//p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15);
@@ -223,11 +229,16 @@ public:
//p.weight = 1.0; // reset
//p.weight = std::pow(p.weight, 0.1); // make all particles a little more equal [less strict]
//p.weight *= std::pow(prob, 0.1); // add grid-walk-probability
p.weight = prob; // grid-walk-probability
p.weight *= prob; // grid-walk-probability
if (p.weight != p.weight) {throw Exception("nan");}
}
// normalize
double weightSum = 0;
for (const auto& p : particles) {weightSum += p.weight;}
for (auto& p : particles) {p.weight /= weightSum;}
}
};
@@ -238,6 +249,8 @@ class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
WiFiModelLogDistCeiling& wifiModel;
EarthMapping& em;
WiFiObserverFree wiFiProbability; // free-calculation
//WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation
@@ -247,8 +260,8 @@ class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
public:
PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel) :
grid(grid), wifiModel(wifiModel),
PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel, EarthMapping& em) :
grid(grid), wifiModel(wifiModel), em(em),
wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free
//wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid
@@ -280,6 +293,7 @@ public:
}
double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override {
double sum = 0;
@@ -292,6 +306,9 @@ public:
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi);
const int numAP2 = wifiObs.entries.size();
// GPS
const GPSProbability gpsProb(em);
Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2));
// sanity check
@@ -301,6 +318,8 @@ public:
for (int i = 0; i < Settings::numParticles; ++i) {
K::Particle<MyState>& p = particles[i];
const MyGridNode& node = grid->getNodeFor(p.state.position);
// WiFi free
const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, wifiObs);
@@ -309,12 +328,13 @@ public:
//const MyGridNode& node = grid->getNodeFor(p.state.position);
//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 prob = pWiFi * pGPS;
//Log::add("xxx", std::to_string(observation.currentTime.ms()) + "_" + std::to_string(wifiObs.entries[0].ts.ms()));
const double pStair = 1;//getStairProb(p, observation.activity);
const double pGPS = 1;
const double prob = pWiFi * pGPS * pStair;
if (pGPS != 1) {
int i = 0; (void) i;
}
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