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

added per-floor optimization

Browse Source
This commit is contained in:
k-a-z-u
2018-05-16 13:05:17 +02:00
parent 4b90ba9ca4
commit 1e52b377dc
3 changed files with 89 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
navMesh/filter.h
View File

@@ -172,10 +172,10 @@ 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>;
MyNavMeshWalk walker;
WiFiQualityAnalyzer analyzer;
@@ -219,20 +219,20 @@ public:
}
// 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){
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;
@@ -262,8 +262,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;
@@ -310,7 +310,10 @@ class MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
public:
MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){}
// FRANK
//MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){}
//MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel, WiFiObserverFree::EvalDist::EXPONENTIAL){}
MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){}
virtual double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& observation) override {
@@ -318,8 +321,8 @@ public:
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
#pragma omp parallel for num_threads(3)
for (size_t i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);