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

code for journal presentation

Browse Source
This commit is contained in:
toni
2018-05-15 09:32:39 +02:00
parent 97ce18f5f1
commit a536b1daec
7 changed files with 165 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
navMesh/filter.h
View File

@@ -16,13 +16,23 @@
#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/navMesh/walk/NavMeshWalkWifi.h>
#include <Indoor/navMesh/walk/NavMeshWalkWifiRegional.h>
#include <Indoor/navMesh/walk/NavMeshWalkUnblockable.h>
#include <Indoor/navMesh/walk/NavMeshWalkKLD.h>
#include <Indoor/navMesh/NavMeshRandom.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/activity/ActivityDetector.h>
#include <Indoor/math/divergence/KullbackLeibler.h>
#include <Indoor/sensors/radio/WiFiQualityAnalyzer.h>
struct MyState {
/** the state's position (within the mesh) */
@@ -83,6 +93,14 @@ struct MyControl {
headingChangeSinceLastEval = 0;
}
//wifi
WiFiMeasurements wifi;
//time
Timestamp currentTime;
//last estimation
Point3 lastEstimate = Point3(26, 43, 7.5);
};
@@ -154,26 +172,75 @@ public:
class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<MyNavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<MyNavMeshTriangle>;
using MyNavMeshWalk = NM::NavMeshWalkKLD<MyNavMeshTriangle>;
MyNavMeshWalk walker;
WiFiQualityAnalyzer analyzer;
WiFiObserverFree wifiProbability;
const double lambda = 0.0003;
SMC::ParticleFilterEstimationBoxKDE<MyState> estimator;
public:
MyPFTrans(MyNavMesh& mesh) : walker(mesh) {
MyPFTrans(MyNavMesh& mesh, WiFiModel& wifiModel) :
walker(mesh),
wifiProbability(Settings::WiFiModel::sigma, wifiModel){
// how to evaluate drawn points
walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
//walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
//walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
//walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
}
estimator = SMC::ParticleFilterEstimationBoxKDE<MyState>(&mesh, 0.2, Point2(1,1));
}
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
// walking and heading random
Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
Distribution::Normal<float> dHeading(0.0, 0.1);
// wifi and quality of wifi
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(control->wifi);
if(!wifiObs.entries.empty()){
analyzer.add(wifiObs);
}
float qualityWifi = analyzer.getQuality();
if(std::isnan(qualityWifi)){
qualityWifi = 1.0;
} else if(qualityWifi == 0) {
qualityWifi = 0.00000001;
}
// divergence between eval and transition
std::vector<SMC::Particle<MyState>> wifiParticles;
NM::NavMeshRandom<MyNavMeshTriangle> rnd = walker.getMesh().getRandom();
for(int i = 0; i < 10000; ++i){
NM::NavMeshLocation<MyNavMeshTriangle> tmpLocation = rnd.draw();
double weight = wifiProbability.getProbability(tmpLocation.pos, control->currentTime, wifiObs);
SMC::Particle<MyState> tmpParticle(MyState(tmpLocation.pos), weight);
wifiParticles.push_back(tmpParticle);
}
MyState wifiEstimate = estimator.estimate(wifiParticles);
// fake kld
const double kld = control->lastEstimate.getDistance(wifiEstimate.pos.pos);
//const double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);;
//std::cout << "KLD: " << kld << std::endl;
//std::cout << "Quality: " << qualityWifi << std::endl;
//update wifi
//walker.updateWiFi(wifiObs, control->currentTime, control->lastEstimate);
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
@@ -192,8 +259,11 @@ public:
params.stepSizes.stepSizeStair_m = 0.1;
}
double deltaUnblockable = 0.01;
// walk
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
//MyNavMeshWalk::ResultEntry res = walker.getOne(params);
MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi);
// assign back to particle's state
p.weight *= res.probability;
@@ -257,9 +327,7 @@ public:
//HACK HACK HACK HACK
double prob = 1.0;
if(observation.currentTime.ms() > 20801){
prob = pWifi * pStair;
}
prob = pWifi * pStair;
p.weight *= prob;

1
navMesh/mesh.h
View File

@@ -18,7 +18,6 @@ class MyNavMeshTriangle : public NM::NavMeshTriangle, public NM::NavMeshTriangle
public:
MyNavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, uint8_t type) : NM::NavMeshTriangle(p1, p2, p3, type) {
;
}
};