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

aktuell spielerein

Browse Source
This commit is contained in:
toni
2018-08-01 10:08:33 +02:00
parent 1e52b377dc
commit 38042f8dad
5 changed files with 145 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
navMesh/filter.h
View File

@@ -23,6 +23,7 @@
#include <Indoor/navMesh/walk/NavMeshWalkWifiRegional.h>
#include <Indoor/navMesh/walk/NavMeshWalkUnblockable.h>
#include <Indoor/navMesh/walk/NavMeshWalkKLD.h>
#include <Indoor/navMesh/walk/NavMeshWalkSinkOrSwim.h>
#include <Indoor/navMesh/NavMeshRandom.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h>
@@ -71,6 +72,11 @@ struct MyState {
return pos.pos.y;
}
float getZ() {
return pos.pos.z;
}
float getBinValue(const int dim) const {
switch (dim) {
case 0: return this->pos.pos.x;
@@ -172,27 +178,31 @@ 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>;
//using MyNavMeshWalk = NM::NavMeshWalkKLD<MyNavMeshTriangle>;
using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<MyNavMeshTriangle>;
MyNavMeshWalk walker;
WiFiQualityAnalyzer analyzer;
WiFiObserverFree wifiProbability;
const double lambda = 0.0003;
const double lambda = 0.03;
SMC::ParticleFilterEstimationBoxKDE<MyState> estimator;
public:
//std::vector<double> listRadiusSub;
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));
@@ -203,36 +213,36 @@ public:
// walking and heading random
Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
Distribution::Normal<float> dStepSizeStair(0.35, 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;
}
// // 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){
// // 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);
}
// 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);
// MyState wifiEstimate = estimator.estimate(wifiParticles);
// fake kld
const double kld = control->lastEstimate.getDistance(wifiEstimate.pos.pos);
// // 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;
@@ -241,6 +251,8 @@ public:
//update wifi
//walker.updateWiFi(wifiObs, control->currentTime, control->lastEstimate);
// listRadiusSub.push_back(kld * qualityWifi);
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
@@ -262,8 +274,8 @@ public:
double deltaUnblockable = 0.01;
// walk
//MyNavMeshWalk::ResultEntry res = walker.getOne(params);
MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi);
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;