FHWS/museumLoc
Archived
3
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

strange bug... a lot of zero weight particles

Browse Source
This commit is contained in:
toni
2018-02-09 17:05:23 +01:00
parent de2570cc0c
commit 46abeae148
5 changed files with 196 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

90
navMesh/filter.h
View File

@@ -2,20 +2,26 @@
#define NAV_MESH_FILTER_H
#include "mesh.h"
#include "../Settings.h"
#include <Indoor/geo/Heading.h>
#include <Indoor/math/Distributions.h>
#include <KLib/math/filter/particles/Particle.h>
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <Indoor/smc/Particle.h>
#include <Indoor/smc/filtering/ParticleFilter.h>
#include <Indoor/smc/filtering/ParticleFilterInitializer.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKLD.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include <Indoor/navMesh/walk/NavMeshWalkEval.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
struct MyState {
/** the state's position (within the mesh) */
@@ -44,6 +50,16 @@ struct MyState {
return res;
}
float getBinValue(const int dim) const {
switch (dim) {
case 0: return this->pos.pos.x;
case 1: return this->pos.pos.y;
case 2: return this->pos.pos.z;
case 3: return this->heading.getRAD();
}
throw "cant find this value within the bin";
}
};
struct MyControl {
@@ -61,11 +77,19 @@ struct MyControl {
struct MyObservation {
// pressure
float sigmaPressure = 0.10f;
float relativePressure = 0;
//wifi
WiFiMeasurements wifi;
//time
Timestamp currentTime;
};
class MyPFInitUniform : public K::ParticleFilterInitializer<MyState> {
class MyPFInitUniform : public SMC::ParticleFilterInitializer<MyState> {
const MyNavMesh* mesh;
@@ -75,12 +99,12 @@ public:
;
}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
/** random position and heading within the mesh */
Distribution::Uniform<float> dHead(0, 2*M_PI);
MyNavMeshRandom rnd = mesh->getRandom();
for (K::Particle<MyState>& p : particles) {
for (SMC::Particle<MyState>& p : particles) {
p.state.pos = rnd.draw();
p.state.heading = dHead.draw();
p.weight = 1.0 / particles.size();
@@ -89,7 +113,7 @@ public:
};
class MyPFInitFixed : public K::ParticleFilterInitializer<MyState> {
class MyPFInitFixed : public SMC::ParticleFilterInitializer<MyState> {
const MyNavMesh* mesh;
const Point3 pos;
@@ -100,11 +124,11 @@ public:
;
}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
/** random position and heading within the mesh */
Distribution::Uniform<float> dHead(0, 2*M_PI);
for (K::Particle<MyState>& p : particles) {
for (SMC::Particle<MyState>& p : particles) {
p.state.pos = mesh->getLocation(pos);
p.state.heading = dHead.draw();
p.weight = 1.0 / particles.size();
@@ -114,7 +138,7 @@ public:
};
class MyPFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
MyNavMeshWalk walker;
@@ -124,20 +148,20 @@ public:
MyPFTrans(MyNavMesh& mesh) : walker(mesh) {
// how to evaluate drawn points
//walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
//walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
//walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
}
void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
Distribution::Normal<float> dHeading(0.0, 0.10);
for (K::Particle<MyState>& p : particles) {
for (SMC::Particle<MyState>& p : particles) {
// how to walk
MyNavMeshWalkParams params;
@@ -148,10 +172,10 @@ public:
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
// walk
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
// assign back to particle's state
p.weight *= res.probability;
p.weight *= res.probability;
p.state.pos = res.location;
p.state.heading = res.heading;
@@ -165,20 +189,38 @@ public:
};
class MyPFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
class MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
WiFiModel& wifiModel;
WiFiObserverFree wifiProbability;
public:
virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& observation) override {
MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){}
return 1.0;
virtual double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& observation) override {
double sum = 0;
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
for (SMC::Particle<MyState>& p : particles) {
double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);
const double prob = pWifi;
p.weight *= prob;
sum += prob;
}
return sum;
}
};
using MyFilter = K::ParticleFilter<MyState, MyControl, MyObservation>;
using MyFilter = SMC::ParticleFilter<MyState, MyControl, MyObservation>;
#endif