228 lines
8.9 KiB
C++
228 lines
8.9 KiB
C++
#ifndef FILTERMESH_
|
|
#define FILTERMESH_
|
|
|
|
#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/resampling/ParticleFilterResamplingSimpleImpoverishment.h>
|
|
|
|
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h>
|
|
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
|
|
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
|
|
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.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>
|
|
|
|
#include "State.h"
|
|
#include "../Observation.h"
|
|
#include "../../Settings.h"
|
|
|
|
#include <omp.h>
|
|
#include <future>
|
|
|
|
namespace MeshBased {
|
|
|
|
class PFInit : public SMC::ParticleFilterInitializer<MyState> {
|
|
|
|
private:
|
|
|
|
const NM::NavMesh<NM::NavMeshTriangle>* mesh;
|
|
|
|
public:
|
|
|
|
PFInit(const NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) {
|
|
;
|
|
}
|
|
|
|
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.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);
|
|
// }
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
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;
|
|
|
|
// local, static control-data COPY
|
|
MyControl ctrl;
|
|
|
|
public:
|
|
|
|
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
|
|
|
|
}
|
|
|
|
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);
|
|
|
|
#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;
|
|
|
|
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;
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
|
|
|
|
WiFiModel& wifiModel;
|
|
WiFiObserverFree wifiProbability;
|
|
|
|
BeaconModel& bleModel;
|
|
BeaconObserverFree bleProbability;
|
|
|
|
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
|
|
|
|
const float kappa = 0.9;
|
|
|
|
switch (act) {
|
|
|
|
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.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;
|
|
}
|
|
|
|
public:
|
|
|
|
//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 evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {
|
|
|
|
double sum = 0;
|
|
|
|
// local copy!! observation might be changed async outside!! (will really produces crashes!)
|
|
const MyObservation observation = _observation;
|
|
|
|
// vap-grouping
|
|
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
|
|
const BeaconMeasurements beaconObs = Settings::BLEModel::vg_eval.group(observation.ble);
|
|
|
|
//TODO: do i need a vap grouping for beacons?
|
|
|
|
// sanity check
|
|
//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];
|
|
|
|
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;
|
|
|
|
//TODO: reduziere das gewicht von partikelen die durch sample imp. oder was anderes sehr weit gesprungen sind.
|
|
|
|
const double prob = pBLE * pStair * pGPS * pWifi;
|
|
|
|
p.weight *= prob;
|
|
if (p.weight != p.weight) {throw Exception("nan");}
|
|
|
|
#pragma omp atomic
|
|
sum += p.weight;
|
|
}
|
|
|
|
return sum;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
}
|
|
|
|
#endif // FILTERMESH_
|