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

version for plotting the figures of kde paper

Browse Source
This commit is contained in:
toni
2018-02-19 18:18:45 +01:00
parent 46abeae148
commit 97ce18f5f1
12 changed files with 1374 additions and 1576 deletions
Download Patch File Download Diff File Expand all files Collapse all files

72
navMesh/filter.h
View File

@@ -3,6 +3,7 @@
#include "mesh.h"
#include "../Settings.h"
#include <omp.h>
#include <Indoor/geo/Heading.h>
#include <Indoor/math/Distributions.h>
@@ -12,15 +13,15 @@
#include <Indoor/smc/filtering/ParticleFilterInitializer.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKLD.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include <Indoor/navMesh/walk/NavMeshWalkEval.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/activity/ActivityDetector.h>
struct MyState {
@@ -32,6 +33,8 @@ struct MyState {
MyState() : pos(), heading(0) {;}
MyState(Point3 p) : pos(p, nullptr), heading(0){;}
MyState& operator += (const MyState& o) {
pos.tria = nullptr; // impossible
pos.pos += o.pos.pos;
@@ -50,6 +53,14 @@ struct MyState {
return res;
}
float getX(){
return pos.pos.x;
}
float getY() {
return pos.pos.y;
}
float getBinValue(const int dim) const {
switch (dim) {
case 0: return this->pos.pos.x;
@@ -87,6 +98,9 @@ struct MyObservation {
//time
Timestamp currentTime;
//activity
Activity activity;
};
class MyPFInitUniform : public SMC::ParticleFilterInitializer<MyState> {
@@ -156,21 +170,28 @@ public:
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
Distribution::Normal<float> dHeading(0.0, 0.10);
Distribution::Normal<float> dHeading(0.0, 0.1);
for (SMC::Particle<MyState>& p : particles) {
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
// how to walk
MyNavMeshWalkParams params;
params.heading = p.state.heading + control->headingChangeSinceLastEval + dHeading.draw();
params.numSteps = control->numStepsSinceLastEval;
params.start = p.state.pos;
params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
if(params.stepSizes.stepSizeFloor_m < 0.1 || params.stepSizes.stepSizeStair_m < 0.1){
params.stepSizes.stepSizeFloor_m = 0.1;
params.stepSizes.stepSizeStair_m = 0.1;
}
// walk
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
@@ -194,6 +215,29 @@ class MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
WiFiModel& wifiModel;
WiFiObserverFree wifiProbability;
//TODO: add this to transition probability
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
const float kappa = 0.75;
switch (act) {
case Activity::WALKING:
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
{return 1-kappa;}
case Activity::WALKING_UP:
case Activity::WALKING_DOWN:
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;}
{return 1-kappa;}
}
return 1.0;
}
public:
MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){}
@@ -203,13 +247,23 @@ public:
double sum = 0;
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
for (SMC::Particle<MyState>& p : particles) {
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);
double pStair = getStairProb(p, observation.activity);
const double prob = pWifi;
//HACK HACK HACK HACK
double prob = 1.0;
if(observation.currentTime.ms() > 20801){
prob = pWifi * pStair;
}
p.weight *= prob;
#pragma omp atomic
sum += prob;
}