FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

changes in kld walker

Browse Source
This commit is contained in:
toni
2018-07-12 11:11:11 +02:00
parent b50995ffe6
commit d96f0588c2
5 changed files with 23 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h
View File

@@ -46,7 +46,7 @@ namespace SMC {
// compile-time sanity checks
// TODO: this solution requires EXPLICIT overloading which is bad...
// static_assert( HasOperatorAssign<State>::value, "your state needs an assignment operator!" );
// static_assert( HasOperatorAssign<State>::value, "your state needs an assignment operator!" );
const uint32_t cnt = (uint32_t) particles.size();
@@ -67,20 +67,21 @@ namespace SMC {
particlesCopy[i].weight = cumWeight;
}
// randomness for drawing particles
std::uniform_real_distribution<float> distNewOne(0.0, 1.0);
// randomness for drawing particles
std::uniform_real_distribution<float> distNewOne(0.0, 1.0);
// now draw from the copy vector and fill the original one
// with the resampled particle-set
for (uint32_t i = 0; i < cnt; ++i) {
// slight chance to get a truely particle in range X m
if (distNewOne(gen) < 0.001) {
const NM::NavMeshSub<Tria> reachable(particlesCopy[i].state.pos, 10.0);
particles[i].state.pos = reachable.getRandom().drawWithin(particlesCopy[i].state.pos.pos, 10.0);
particles[i].weight = equalWeight;
continue;
}
// slight chance to get a truely random particle in range X m
if (distNewOne(gen) < 0.001) {
const double radius = 50.0;
const NM::NavMeshSub<Tria> reachable(particlesCopy[i].state.pos, radius);
particles[i].state.pos = reachable.getRandom().drawWithin(particlesCopy[i].state.pos.pos, radius);
particles[i].weight = equalWeight;
continue;
}
particles[i] = draw(cumWeight);
particles[i].weight = equalWeight;