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

current state

Browse Source
This commit is contained in:
FrankE
2017-04-18 18:03:31 +02:00
parent 3e244118cc
commit 11cb939758
18 changed files with 1809 additions and 516 deletions
Download Patch File Download Diff File Expand all files Collapse all files

144
pf/PF.h
View File

@@ -12,7 +12,8 @@
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
#include <Indoor/sensors/radio/WiFiQualityAnalyzer.h>
#include <Indoor/sensors/gps/GPSData.h>
#include <Indoor/sensors/gps/GPSProbability.h>
#include <Indoor/sensors/activity/Activity.h>
@@ -92,6 +93,9 @@ struct MyObservation {
/** gps measurements */
GPSData gps;
/** absolute heading in radians */
float compassAzimuth_rad = 0;
// TODO: switch to a general activity enum/detector for barometer + accelerometer + ...?
/** detected activity */
ActivityButterPressure::Activity activity;
@@ -181,9 +185,8 @@ public:
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);
//walker.addModule(&modAbsHead);
//walker.addModule(&modActivity);
//walker.addModule(&modFavorZ);
@@ -197,40 +200,77 @@ public:
}
int numTrans = 0;
void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* _ctrl) override {
++numTrans;
// local copy!! observation might be changed async outside!! (will really produces crashes!)
this->ctrl = *_ctrl;
((MyControl*)_ctrl)->resetAfterTransition();
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
// std::normal_distribution<float> turnNoise (0, 0.2);
// std::uniform_real_distribution<float> initTurnNoise(-0.3, +0.3);
// std::uniform_real_distribution<float> initDistNoise(0.0, 0.5);
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
//for (K::Particle<MyState>& p : particles) {
//#pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) {
//#pragma omp atomic
const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
K::Particle<MyState>& p = particles[i];
p.weight = std::pow(p.weight, 0.5);
// first transitions: more variation as the state is unknown
if (numTrans < 50 || numTrans % 10 == 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);
const int idx = distIdx(gen);
n = &(grid->getNeighbor(*n, idx));
}
p.state.position = *n;
if (numTrans < 50) {
p.state.heading.direction += distTurn(gen);
dist_m += 0.5;
}
}
//p.state.heading.direction += turnNoise(gen);
// "blur" the density by adjusting the weights
//p.weight = std::pow(p.weight, 0.4);
//p.weight = 1;
double prob;
double prob = 1.0;
p.state = walker.getDestination(*grid, p.state, dist_m, prob);
//p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15);
//p.weight = (prob > 0.01) ? (1.0) : (0.15);
//p.weight = prob;
//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.7); // add grid-walk-probability
p.weight *= prob; // grid-walk-probability
if (p.weight != p.weight) {throw Exception("nan");}
if (p.weight != p.weight) {
throw Exception("nan");
}
if (p.weight == 0) {
std::cout << "one particle weight is 0.0!" << std::endl;
//throw Exception("weight = 0.0");
}
}
@@ -253,7 +293,6 @@ class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
WiFiObserverFree wiFiProbability; // free-calculation
//WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation
// smartphone is 1.3 meter above ground
const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);
@@ -264,88 +303,109 @@ public:
grid(grid), wifiModel(wifiModel), em(em),
wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free
//wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid
}
double getStairProb(const K::Particle<MyState>& p, const ActivityButterPressure::Activity act) {
const float kappa = 0.75;
double getAbsHead(const MyState& s, const MyObservation& obs) {
const MyGridNode& gn = grid->getNodeFor(p.state.position);
switch (act) {
// compare the heading against the state's heading - the last error
const Heading stateHead = s.heading.direction;
const Heading obsHead(obs.compassAzimuth_rad);
case ActivityButterPressure::Activity::STAY:
if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;}
if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;}
{return 1-kappa;}
// get the difference
const float angularDiff = obsHead.getDiffHalfRAD(stateHead);
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;}
double res = 0;
}
if (angularDiff > Angle::degToRad(90)) {res = 0.05;}
else if (angularDiff > Angle::degToRad(45)) {res = 0.25;}
else {res = 0.70;}
return 1.0;
return res;
}
WiFiQualityAnalyzer wqa;
double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override {
double sum = 0;
// local copy!! observation might be changed async outside!! (will really produces crashes!)
const MyObservation observation = _observation;
// vap-grouping
const int numAP1 = observation.wifi.entries.size();
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi);
WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi);
const int numAP2 = wifiObs.entries.size();
Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2));
wqa.add(wifiObs);
const float quality = wqa.getQuality();
// GPS
const GPSProbability gpsProb(em);
Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2));
// sanity check
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);
//#pragma omp parallel for num_threads(3)
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);
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);
if (quality < 0.2) {
pWiFi = 1;
pWiFiVeto = 1;
std::cout << "disabling WiFi" << std::endl;
}
// WiFi grid
//const MyGridNode& node = grid->getNodeFor(p.state.position);
//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;
const double pAbsHead = getAbsHead(p.state, observation);
const double prob = pWiFi * pWiFiVeto;// * pAbsHead;
//GPS ERROR?!?!?! does it work without disabling wifi when gps is disabled?
if (pGPS != 1) {
int i = 0; (void) i;
}
// TESTING
//p.weight = std::pow(p.weight, 0.5);
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
if (p.weight != p.weight) {throw Exception("nan");}
#pragma omp atomic
sum += p.weight;
if (p.weight != p.weight) {
throw Exception("nan");
}
if (p.weight == 0) {
std::cout << "one particle weight is 0.0!" << std::endl;
// throw Exception("weight = 0.0");
}
}
return sum;
return 0;
}