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

fixed some minor bugs using ble on android

Browse Source 
added BLE to the evaluation of the particle filter
made first evaluation inside fhws
This commit is contained in:
mail@toni-fetzer.de
2019-06-10 16:58:24 +02:00
parent ef6066ae14
commit 7fe0f22a6e
9 changed files with 311 additions and 231 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
nav/NavController.h
View File

@@ -5,6 +5,7 @@
#include "../sensors/GyroscopeSensor.h"
#include "../sensors/BarometerSensor.h"
#include "../sensors/WiFiSensor.h"
#include "../sensors/BLESensor.h"
#include "../sensors/SensorFactory.h"
#include "../sensors/StepSensor.h"
#include "../sensors/TurnSensor.h"

258
nav/mesh/FilterMesh.h
View File

@@ -14,11 +14,14 @@
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
#include <Indoor/sensors/beacon/model/BeaconModel.h>
#include <Indoor/sensors/beacon/BeaconProbabilityFree.h>
#include <Indoor/sensors/beacon/BeaconMeasurementGrouper.h>
#include <Indoor/navMesh/NavMesh.h>
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include <Indoor/navMesh/walk/NavMeshWalkSinkOrSwim.h>
@@ -32,185 +35,192 @@
namespace MeshBased {
class PFInit : public SMC::ParticleFilterInitializer<MyState> {
class PFInit : public SMC::ParticleFilterInitializer<MyState> {
private:
private:
const NM::NavMesh<NM::NavMeshTriangle>* mesh;
const NM::NavMesh<NM::NavMeshTriangle>* mesh;
public:
public:
PFInit(const NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) {
;
}
PFInit(const NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) {
;
}
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
std::minstd_rand gen;
std::uniform_real_distribution<float> distHead(0, 2*M_PI);
std::minstd_rand gen;
std::uniform_real_distribution<float> distHead(0, 2*M_PI);
NM::NavMeshRandom<NM::NavMeshTriangle> rnd = mesh->getRandom();
NM::NavMeshRandom<NM::NavMeshTriangle> rnd = mesh->getRandom();
for (SMC::Particle<MyState>& p : particles) {
p.state.pos = rnd.draw();
p.state.heading = Heading(distHead(gen)); // random heading
p.weight = 1.0 / particles.size(); // equal weight
}
for (SMC::Particle<MyState>& p : particles) {
p.state.pos = rnd.draw();
p.state.heading = Heading(distHead(gen)); // random heading
p.weight = 1.0 / particles.size(); // equal weight
}
// // fix position + heading
// for (SMC::Particle<MyState>& p : particles) {
//// const int idx = 9000;
//// const MyGridNode& node = (*grid)[idx];
// const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap
// p.state.position = node;
// p.state.heading.direction = Heading(0);
// }
// // fix position + heading
// for (SMC::Particle<MyState>& p : particles) {
//// const int idx = 9000;
//// const MyGridNode& node = (*grid)[idx];
// const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap
// p.state.position = node;
// p.state.heading.direction = Heading(0);
// }
}
}
};
};
class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
public:
public:
//using MyNavMeshWalk = NM::NavMeshWalkSimple<NM::NavMeshTriangle>;
using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<NM::NavMeshTriangle>;
MyNavMeshWalk walker;
//using MyNavMeshWalk = NM::NavMeshWalkSimple<NM::NavMeshTriangle>;
using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<NM::NavMeshTriangle>;
MyNavMeshWalk walker;
// local, static control-data COPY
MyControl ctrl;
// local, static control-data COPY
MyControl ctrl;
public:
public:
PFTrans(NM::NavMesh<NM::NavMeshTriangle>* mesh) : walker(*mesh){
PFTrans(NM::NavMesh<NM::NavMeshTriangle>* mesh) : walker(*mesh){
// how to evaluate drawn points
walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<NM::NavMeshTriangle>(0.04));
walker.addEvaluator(new NM::WalkEvalDistance<NM::NavMeshTriangle>(0.1));
//walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
// how to evaluate drawn points
walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<NM::NavMeshTriangle>(0.04));
walker.addEvaluator(new NM::WalkEvalDistance<NM::NavMeshTriangle>(0.1));
//walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
}
}
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* _ctrl) override {
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* _ctrl) override {
// local copy!! observation might be changed async outside!! (will really produces crashes!)
this->ctrl = *_ctrl;
((MyControl*)_ctrl)->resetAfterTransition();
// local copy!! observation might be changed async outside!! (will really produces crashes!)
this->ctrl = *_ctrl;
((MyControl*)_ctrl)->resetAfterTransition();
// walking and heading random
Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
Distribution::Normal<float> dHeading(0.0, 0.1);
// walking and heading random
Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
Distribution::Normal<float> dHeading(0.0, 0.1);
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
// how to walk
NM::NavMeshWalkParams<NM::NavMeshTriangle> params;
params.heading = p.state.heading + ctrl.turnSinceLastTransition_rad + dHeading.draw();
params.numSteps = ctrl.numStepsSinceLastTransition;
params.start = p.state.pos;
// how to walk
NM::NavMeshWalkParams<NM::NavMeshTriangle> params;
params.heading = p.state.heading + ctrl.turnSinceLastTransition_rad + dHeading.draw();
params.numSteps = ctrl.numStepsSinceLastTransition;
params.start = p.state.pos;
params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
if(params.stepSizes.stepSizeFloor_m < 0.1 || params.stepSizes.stepSizeStair_m < 0.1){
params.stepSizes.stepSizeFloor_m = 0.1;
params.stepSizes.stepSizeStair_m = 0.1;
}
if(params.stepSizes.stepSizeFloor_m < 0.1 || params.stepSizes.stepSizeStair_m < 0.1){
params.stepSizes.stepSizeFloor_m = 0.1;
params.stepSizes.stepSizeStair_m = 0.1;
}
// walk
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
// walk
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
// assign back to particle's state
p.weight *= res.probability;
p.state.pos = res.location;
p.state.heading = res.heading;
}
// assign back to particle's state
p.weight *= res.probability;
p.state.pos = res.location;
p.state.heading = res.heading;
}
}
}
};
};
class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
WiFiModel& wifiModel;
WiFiObserverFree wifiProbability;
WiFiModel& wifiModel;
WiFiObserverFree wifiProbability;
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
BeaconModel& bleModel;
BeaconObserverFree bleProbability;
const float kappa = 0.9;
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
switch (act) {
const float kappa = 0.9;
case Activity::WALKING:
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
{return 1-kappa;}
switch (act) {
case Activity::WALKING_UP:
case Activity::WALKING_DOWN:
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;}
{return 1-kappa;}
}
case Activity::WALKING:
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
{return 1-kappa;}
return 1.0;
}
case Activity::WALKING_UP:
case Activity::WALKING_DOWN:
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;}
{return 1-kappa;}
}
public:
return 1.0;
}
//TODO: Was ist hier besser? Im Museum hatten wir das unterste.
PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel){}
//PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::EXPONENTIAL){}
//PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){}
public:
double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {
//TODO: Was ist hier besser? Im Museum hatten wir das unterste.
PFEval(WiFiModel* wifiModel, BeaconModel* bleModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel), bleModel(*bleModel), bleProbability(Settings::BLEModel::sigma, *bleModel){}
//PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::EXPONENTIAL){}
//PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){}
double sum = 0;
double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {
// local copy!! observation might be changed async outside!! (will really produces crashes!)
const MyObservation observation = _observation;
double sum = 0;
// vap-grouping
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
// local copy!! observation might be changed async outside!! (will really produces crashes!)
const MyObservation observation = _observation;
// sanity check
//Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
// vap-grouping
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
const BeaconMeasurements beaconObs = Settings::BLEModel::vg_eval.group(observation.ble);
// assign weights
#pragma omp parallel for num_threads(3)
for (size_t i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
//TODO: do i need a vap grouping for beacons?
const double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);
const double pStair = getStairProb(p, observation.activity);
const double pGPS = 1;
// sanity check
//Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
//TODO: reduziere das gewicht von partikelen die durch sample imp. oder was anderes sehr weit gesprungen sind.
// assign weights
#pragma omp parallel for num_threads(3)
for (size_t i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
const double prob = pWifi * pStair * pGPS;
const double pBLE = bleProbability.getProbability(p.state.pos.pos, observation.currentTime, beaconObs);
const double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);
const double pStair = getStairProb(p, observation.activity);
const double pGPS = 1;
p.weight *= prob;
if (p.weight != p.weight) {throw Exception("nan");}
//TODO: reduziere das gewicht von partikelen die durch sample imp. oder was anderes sehr weit gesprungen sind.
#pragma omp atomic
sum += p.weight;
}
const double prob = pBLE * pStair * pGPS * pWifi;
return sum;
p.weight *= prob;
if (p.weight != p.weight) {throw Exception("nan");}
}
#pragma omp atomic
sum += p.weight;
}
};
return sum;
}
};
}

8
nav/mesh/NavControllerMesh.cpp
View File

@@ -31,11 +31,13 @@
#include "Settings.h"
#include "../Controller.h"
Q_DECLARE_METATYPE(const void*)
/** ctor */
MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel) :
NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel) {
MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel, BeaconModel* bleModel) :
NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel), bleModel(bleModel) {
// filter init
std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh));
@@ -54,7 +56,7 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
std::unique_ptr<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>());
// eval and transition
std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new MeshBased::PFEval(wifiModel));
std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new MeshBased::PFEval(wifiModel, bleModel));
std::unique_ptr<SMC::ParticleFilterTransition<MyState, MyControl>> transition(new MeshBased::PFTrans(navMesh));
// setup the filter

5
nav/mesh/NavControllerMesh.h
View File

@@ -12,6 +12,7 @@
#include <Indoor/navMesh/NavMeshLocation.h>
#include <Indoor/navMesh/NavMesh.h>
#include <Indoor/sensors/beacon/model/BeaconModel.h>
#include <Indoor/Assertions.h>
#include <thread>
@@ -19,7 +20,6 @@
//#include "State.h"
#include "FilterMesh.h"
#include "../Controller.h"
#include "../NavController.h"
namespace MeshBased {
@@ -30,6 +30,7 @@ namespace MeshBased {
NM::NavMesh<NM::NavMeshTriangle>* navMesh;
WiFiModel* wifiModel;
BeaconModel* bleModel;
std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf;
@@ -38,7 +39,7 @@ namespace MeshBased {
public:
NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel);
NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel, BeaconModel* bleModel);
void start() override;