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

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