started to add ble functions
added ble as sensor to java and c++ added sensorlistener for ble added ble to observation and onDataSensor in filter started to work on ble fingerprints for optimization
This commit is contained in:
mail@toni-fetzer.de
@@ -32,33 +32,33 @@
|
||||
|
||||
namespace MeshBased {
|
||||
|
||||
class PFInit : public SMC::ParticleFilterInitializer<MyState> {
|
||||
class PFInit : public SMC::ParticleFilterInitializer<MyState> {
|
||||
|
||||
private:
|
||||
|
||||
const NM::NavMesh<NM::NavMeshTriangle>* mesh;
|
||||
const NM::NavMesh<NM::NavMeshTriangle>* mesh;
|
||||
|
||||
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);
|
||||
|
||||
NM::NavMeshRandom<NM::NavMeshTriangle> rnd = mesh->getRandom();
|
||||
|
||||
for (SMC::Particle<MyState>& p : particles) {
|
||||
p.state.loc = rnd.draw();
|
||||
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) {
|
||||
// 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
|
||||
@@ -71,109 +71,109 @@ namespace MeshBased {
|
||||
};
|
||||
|
||||
|
||||
class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
|
||||
class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
|
||||
|
||||
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
|
||||
// local, static control-data COPY
|
||||
MyControl ctrl;
|
||||
|
||||
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();
|
||||
|
||||
// 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.loc;
|
||||
// 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.loc = 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) {
|
||||
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
|
||||
|
||||
const float kappa = 0.9;
|
||||
const float kappa = 0.9;
|
||||
|
||||
switch (act) {
|
||||
switch (act) {
|
||||
|
||||
case Activity::WALKING:
|
||||
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;}
|
||||
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;}
|
||||
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {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;}
|
||||
|
||||
case Activity::WALKING_UP:
|
||||
case Activity::WALKING_DOWN:
|
||||
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;}
|
||||
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
|
||||
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;}
|
||||
{return 1-kappa;}
|
||||
}
|
||||
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;}
|
||||
}
|
||||
|
||||
return 1.0;
|
||||
}
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
//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){}
|
||||
//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){}
|
||||
|
||||
double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {
|
||||
double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {
|
||||
|
||||
double sum = 0;
|
||||
|
||||
@@ -181,32 +181,32 @@ namespace MeshBased {
|
||||
const MyObservation observation = _observation;
|
||||
|
||||
// vap-grouping
|
||||
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
|
||||
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
|
||||
|
||||
// sanity check
|
||||
//Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
|
||||
//Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
|
||||
|
||||
// assign weights
|
||||
#pragma omp parallel for num_threads(3)
|
||||
for (size_t i = 0; i < particles.size(); ++i) {
|
||||
SMC::Particle<MyState>& p = particles[i];
|
||||
// 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 pWifi = wifiProbability.getProbability(p.state.loc.pos, observation.currentTime, wifiObs);
|
||||
const double pStair = getStairProb(p, observation.activity);
|
||||
const double pGPS = 1;
|
||||
const double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);
|
||||
const double pStair = getStairProb(p, observation.activity);
|
||||
const double pGPS = 1;
|
||||
|
||||
//TODO: reduziere das gewicht von partikelen die durch sample imp. oder was anderes sehr weit gesprungen sind.
|
||||
//TODO: reduziere das gewicht von partikelen die durch sample imp. oder was anderes sehr weit gesprungen sind.
|
||||
|
||||
const double prob = pWifi * pStair * pGPS;
|
||||
const double prob = pWifi * pStair * pGPS;
|
||||
|
||||
p.weight *= prob;
|
||||
if (p.weight != p.weight) {throw Exception("nan");}
|
||||
p.weight *= prob;
|
||||
if (p.weight != p.weight) {throw Exception("nan");}
|
||||
|
||||
#pragma omp atomic
|
||||
sum += p.weight;
|
||||
}
|
||||
#pragma omp atomic
|
||||
sum += p.weight;
|
||||
}
|
||||
|
||||
return sum;
|
||||
return sum;
|
||||
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user