FHWS/FtmPrologic
Archived
4
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Changed random transition, using weightes avg as estimation, removed ftm offsets from path7

Browse Source
This commit is contained in:
Markus Bullmann
2019-10-08 13:37:29 +02:00
parent 9b9feaa2c2
commit f9d2b10839
4 changed files with 19 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
code/Plotty.h
View File

@@ -614,8 +614,8 @@ public:
// K::GnuplotPoint3(bbox.getMax().x, bbox.getMax().y, bbox.getMax().z) // K::GnuplotPoint3(bbox.getMax().x, bbox.getMax().y, bbox.getMax().z)
// ); // );
splot.getAxisX().setRange(K::GnuplotAxis::Range(bbox.getMin().x, bbox.getMax().x)); splot.getAxisX().setRange(K::GnuplotAxis::Range(bbox.getMin().x-5, bbox.getMax().x+5));
splot.getAxisY().setRange(K::GnuplotAxis::Range(bbox.getMin().y, bbox.getMax().y)); 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)); splot.getAxisZ().setRange(K::GnuplotAxis::Range(0 /*bbox.getMin().z*/, bbox.getMax().z));
// process each selected floor // process each selected floor

12
code/Settings.h
View File

@@ -11,7 +11,7 @@ namespace Settings {
const std::string errorDir = "../measurements/error/"; const std::string errorDir = "../measurements/error/";
const std::string plotDataDir = "../plots/data/"; const std::string plotDataDir = "../plots/data/";
const bool UseKalman = true; const bool UseKalman = false;
/** describes one dataset (map, training, parameter-estimation, ...) */ /** describes one dataset (map, training, parameter-estimation, ...) */
@@ -228,10 +228,10 @@ namespace Settings {
}, },
{ {
// NUC, ID Pos X Y Z offset loss kalman stddev // NUC, ID Pos X Y Z offset loss kalman stddev
{ NUC1, {1, { 54, 46, 0.8}, 5.00, 3.375, 3.0f} }, // NUC 1 { NUC1, {1, { 54, 46, 0.8}, 0.0, 3.375, 3.0f} }, // NUC 1
{ NUC2, {2, { 45, 37, 0.8}, 5.00, 3.375, 3.0f} }, // NUC 2 { NUC2, {2, { 45, 37, 0.8}, 0.0, 3.375, 3.0f} }, // NUC 2
{ NUC3, {3, { 27, 45, 0.8}, 5.00, 3.250, 3.0f} }, // NUC 3 { NUC3, {3, { 27, 45, 0.8}, 0.0, 3.250, 3.0f} }, // NUC 3
{ NUC4, {4, { 16, 36, 0.8}, 5.75, 3.375, 3.0f} }, // NUC 4 { NUC4, {4, { 16, 36, 0.8}, 0.0, 3.375, 3.0f} }, // NUC 4
}, },
{ 100, 102, 103, 104, 105, 104, 103, 102, 100 }, { 100, 102, 103, 104, 105, 104, 103, 102, 100 },
true true
@@ -275,7 +275,7 @@ namespace Settings {
}; };
const DataSetup CurrentPath = Path8; const DataSetup CurrentPath = Path7;
} data; } data;

12
code/filter.h
View File

@@ -180,8 +180,10 @@ class MyPFTransStatic : public SMC::ParticleFilterTransition<MyState, MyControl>
struct MyPFTransRandom : public SMC::ParticleFilterTransition<MyState, MyControl>{ struct MyPFTransRandom : public SMC::ParticleFilterTransition<MyState, MyControl>{
Distribution::Normal<float> dStepSize; Distribution::Normal<float> dStepSize;
Distribution::Uniform<float> dHeading;
MyPFTransRandom() MyPFTransRandom()
: dStepSize(0.0f, 0.6f) : dStepSize(2.0f, 0.2f), dHeading(0, 2*M_PI)
{} {}
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override { void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
@@ -189,8 +191,12 @@ struct MyPFTransRandom : public SMC::ParticleFilterTransition<MyState, MyControl
#pragma omp parallel for num_threads(3) #pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) { for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i]; SMC::Particle<MyState>& 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;
} }
} }
}; };

4
code/main.cpp
View File

@@ -263,8 +263,8 @@ static CombinedStats<float> run(Settings::DataSetup setup, int walkIdx, std::str
//auto trans = std::make_unique<MyPFTransStatic>(); //auto trans = std::make_unique<MyPFTransStatic>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>(); auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>(); auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>(); //auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>();
// setup // setup
MyFilter pf(numParticles, std::move(init)); MyFilter pf(numParticles, std::move(init));