diff --git a/code/filter.h b/code/filter.h index 55deb66..006d473 100644 --- a/code/filter.h +++ b/code/filter.h @@ -180,6 +180,24 @@ class MyPFTransStatic : public SMC::ParticleFilterTransition } }; +struct MyPFTransRandom : public SMC::ParticleFilterTransition{ + + Distribution::Normal dStepSize; + MyPFTransRandom() + : dStepSize(0.0f, 0.6f) + {} + + void transition(std::vector>& particles, const MyControl* control) override { + + #pragma omp parallel for num_threads(3) + for (int i = 0; i < particles.size(); ++i) { + SMC::Particle& p = particles[i]; + p.state.pos.pos.x += dStepSize.draw(); + p.state.pos.pos.y += dStepSize.draw(); + } + } +}; + class MyPFTrans : public SMC::ParticleFilterTransition { //using MyNavMeshWalk = NM::NavMeshWalkSimple; @@ -190,8 +208,6 @@ class MyPFTrans : public SMC::ParticleFilterTransition { MyNavMeshWalk walker; - const double lambda = 0.03; - public: //std::vector listRadiusSub; @@ -208,9 +224,9 @@ public: void transition(std::vector>& particles, const MyControl* control) override { // walking and heading random - Distribution::Normal dStepSizeFloor(0.60, 0.1); - Distribution::Normal dStepSizeStair(0.35, 0.1); - Distribution::Normal dHeading(0.0, 0.1); + Distribution::Normal dStepSizeFloor(0.60f, 0.1f); + Distribution::Normal dStepSizeStair(0.35f, 0.1f); + Distribution::Normal dHeading(0.0, 0.1f); #pragma omp parallel for num_threads(3) for (int i = 0; i < particles.size(); ++i) { @@ -225,16 +241,13 @@ public: 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; + if(params.stepSizes.stepSizeFloor_m < 0.1f || params.stepSizes.stepSizeStair_m < 0.1f){ + params.stepSizes.stepSizeFloor_m = 0.1f; + params.stepSizes.stepSizeStair_m = 0.1f; } - double deltaUnblockable = 0.01; - // walk 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; @@ -303,7 +316,7 @@ public: for (auto& wifi : observation.wifi) { if ( (true && wifi.second.getAP().getMAC() == Settings::NUC1) - || (true && wifi.second.getAP().getMAC() == Settings::NUC2) + || (true && wifi.second.getAP().getMAC() == Settings::NUC2) || (true && wifi.second.getAP().getMAC() == Settings::NUC3) || (true && wifi.second.getAP().getMAC() == Settings::NUC4) ) diff --git a/code/main.cpp b/code/main.cpp index 0fb9d72..882d4a9 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -265,7 +265,7 @@ static Stats::Statistics run(Settings::DataSetup setup, int walkIdx, std: auto eval = std::make_unique(); eval->kalmanMap = kalmanMap; - auto trans = std::make_unique(mesh); + auto trans = std::make_unique(); //auto trans = std::make_unique(); auto resample = std::make_unique>(); @@ -380,7 +380,7 @@ static Stats::Statistics run(Settings::DataSetup setup, int walkIdx, std: ctrl.lastEstimate = est.pos.pos; - // draw wifi ranges + // draw wifi ranges for (auto& ftm : obs.wifi) { int nucid = Settings::data.CurrentPath.NUCs.at(ftm.second.getAP().getMAC()).ID;