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)
// );
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

12
code/Settings.h
View File

@@ -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;

12
code/filter.h
View File

@@ -180,8 +180,10 @@ class MyPFTransStatic : public SMC::ParticleFilterTransition<MyState, MyControl>
struct MyPFTransRandom : public SMC::ParticleFilterTransition<MyState, MyControl>{
Distribution::Normal<float> dStepSize;
Distribution::Uniform<float> dHeading;
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 {
@@ -189,8 +191,12 @@ struct MyPFTransRandom : public SMC::ParticleFilterTransition<MyState, MyControl
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++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 resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>();
auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>();
// setup
MyFilter pf(numParticles, std::move(init));