diff --git a/code/Plotty.h b/code/Plotty.h index 0df0f3c..b76fef5 100644 --- a/code/Plotty.h +++ b/code/Plotty.h @@ -614,8 +614,8 @@ public: // K::GnuplotPoint3(bbox.getMax().x, bbox.getMax().y, bbox.getMax().z) // ); - splot.getAxisX().setRange(K::GnuplotAxis::Range(bbox.getMin().x, bbox.getMax().x)); - splot.getAxisY().setRange(K::GnuplotAxis::Range(bbox.getMin().y, bbox.getMax().y)); + splot.getAxisX().setRange(K::GnuplotAxis::Range(bbox.getMin().x-5, bbox.getMax().x+5)); + splot.getAxisY().setRange(K::GnuplotAxis::Range(bbox.getMin().y-5, bbox.getMax().y+5)); splot.getAxisZ().setRange(K::GnuplotAxis::Range(0 /*bbox.getMin().z*/, bbox.getMax().z)); // process each selected floor diff --git a/code/Settings.h b/code/Settings.h index 1163c53..245602e 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -11,7 +11,7 @@ namespace Settings { const std::string errorDir = "../measurements/error/"; const std::string plotDataDir = "../plots/data/"; - const bool UseKalman = true; + const bool UseKalman = false; /** describes one dataset (map, training, parameter-estimation, ...) */ @@ -228,10 +228,10 @@ namespace Settings { }, { // NUC, ID Pos X Y Z offset loss kalman stddev - { NUC1, {1, { 54, 46, 0.8}, 5.00, 3.375, 3.0f} }, // NUC 1 - { NUC2, {2, { 45, 37, 0.8}, 5.00, 3.375, 3.0f} }, // NUC 2 - { NUC3, {3, { 27, 45, 0.8}, 5.00, 3.250, 3.0f} }, // NUC 3 - { NUC4, {4, { 16, 36, 0.8}, 5.75, 3.375, 3.0f} }, // NUC 4 + { NUC1, {1, { 54, 46, 0.8}, 0.0, 3.375, 3.0f} }, // NUC 1 + { NUC2, {2, { 45, 37, 0.8}, 0.0, 3.375, 3.0f} }, // NUC 2 + { NUC3, {3, { 27, 45, 0.8}, 0.0, 3.250, 3.0f} }, // NUC 3 + { NUC4, {4, { 16, 36, 0.8}, 0.0, 3.375, 3.0f} }, // NUC 4 }, { 100, 102, 103, 104, 105, 104, 103, 102, 100 }, true @@ -275,7 +275,7 @@ namespace Settings { }; - const DataSetup CurrentPath = Path8; + const DataSetup CurrentPath = Path7; } data; diff --git a/code/filter.h b/code/filter.h index fdff44d..d2ebd66 100644 --- a/code/filter.h +++ b/code/filter.h @@ -180,8 +180,10 @@ class MyPFTransStatic : public SMC::ParticleFilterTransition struct MyPFTransRandom : public SMC::ParticleFilterTransition{ Distribution::Normal dStepSize; + Distribution::Uniform dHeading; + MyPFTransRandom() - : dStepSize(0.0f, 0.6f) + : dStepSize(2.0f, 0.2f), dHeading(0, 2*M_PI) {} void transition(std::vector>& particles, const MyControl* control) override { @@ -189,8 +191,12 @@ struct MyPFTransRandom : public SMC::ParticleFilterTransition& p = particles[i]; - p.state.pos.pos.x += dStepSize.draw(); - p.state.pos.pos.y += dStepSize.draw(); + + const float angle = dHeading.draw(); + const float stepSize = dStepSize.draw(); + + p.state.pos.pos.x += std::cos(angle) * stepSize; + p.state.pos.pos.y += std::sin(angle) * stepSize; } } }; diff --git a/code/main.cpp b/code/main.cpp index 2b93956..e2db731 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -263,8 +263,8 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str //auto trans = std::make_unique(); auto resample = std::make_unique>(); - //auto estimate = std::make_unique>(); - auto estimate = std::make_unique>(); + auto estimate = std::make_unique>(); + //auto estimate = std::make_unique>(); // setup MyFilter pf(numParticles, std::move(init));