625f5fe04d
removed KLib stuff added new activity filter is uncommand! at the moment, the app is not able to load new maps and breaks using old maps
259 lines
8.0 KiB
C++
259 lines
8.0 KiB
C++
#ifndef FILTER_H
|
|
#define FILTER_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/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/WiFiProbabilityFree.h>
|
|
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
|
|
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
|
|
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
|
|
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
|
|
|
|
#include "../Observation.h"
|
|
#include "State.h"
|
|
#include "Node.h"
|
|
#include "NodeResampling.h"
|
|
#include "../Settings.h"
|
|
|
|
#include <omp.h>
|
|
#include <future>
|
|
|
|
namespace GridBased {
|
|
|
|
class PFInit : public SMC::ParticleFilterInitializer<MyState> {
|
|
|
|
private:
|
|
|
|
Grid<MyGridNode>* grid;
|
|
|
|
public:
|
|
|
|
PFInit(Grid<MyGridNode>* grid) : grid(grid) {
|
|
|
|
}
|
|
|
|
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
|
|
|
|
std::minstd_rand gen;
|
|
std::uniform_int_distribution<int> distIdx(0, grid->getNumNodes()-1);
|
|
std::uniform_real_distribution<float> distHead(0, 2*M_PI);
|
|
|
|
|
|
for (SMC::Particle<MyState>& p : particles) {
|
|
const int idx = distIdx(gen);
|
|
const MyGridNode& node = (*grid)[idx];
|
|
p.state.position = node; // random position
|
|
p.state.heading.direction = 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:
|
|
|
|
/** local, static control-data COPY */
|
|
MyControl ctrl;
|
|
|
|
Grid<MyGridNode>* grid;
|
|
GridWalker<MyGridNode, MyState> walker;
|
|
|
|
WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
|
|
WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading;
|
|
WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
|
|
WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
|
|
WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
|
|
|
|
NodeResampling<MyState, MyGridNode> resampler;
|
|
|
|
std::minstd_rand gen;
|
|
|
|
public:
|
|
|
|
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl), resampler(*grid) {
|
|
|
|
//walker.addModule(&modFavorZ);
|
|
walker.addModule(&modHeading);
|
|
//walker.addModule(&modImportance);
|
|
walker.addModule(&modActivity);
|
|
|
|
|
|
if (Settings::destination != GridPoint(0,0,0)) {
|
|
//walker.addModule(&modDestination);
|
|
modDestination.setDestination(grid->getNodeFor(Settings::destination));
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
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();
|
|
|
|
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
|
|
|
|
// sanity check
|
|
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
|
|
|
|
//for (SMC::Particle<MyState>& p : particles) {
|
|
#pragma omp parallel for num_threads(3)
|
|
for (int i = 0; i < Settings::numParticles; ++i) {
|
|
|
|
//#pragma omp atomic
|
|
const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
|
|
|
|
SMC::Particle<MyState>& p = particles[i];
|
|
|
|
double prob;
|
|
p.state = walker.getDestination(*grid, p.state, dist_m, prob);
|
|
//p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15);
|
|
//p.weight = (prob > 0.01) ? (1.0) : (0.15);
|
|
//p.weight = prob;
|
|
//p.weight = 1.0; // reset
|
|
//p.weight = std::pow(p.weight, 0.1); // make all particles a little more equal [less strict]
|
|
//p.weight *= std::pow(prob, 0.1); // add grid-walk-probability
|
|
p.weight = prob; // grid-walk-probability
|
|
if (p.weight != p.weight) {throw Exception("nan");}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
|
|
|
|
Grid<MyGridNode>* grid;
|
|
|
|
WiFiModelLogDistCeiling& wifiModel;
|
|
|
|
|
|
//WiFiObserverFree wiFiProbability; // free-calculation
|
|
WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation
|
|
|
|
// smartphone is 1.3 meter above ground
|
|
const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);
|
|
|
|
public:
|
|
|
|
PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel) :
|
|
grid(grid), wifiModel(wifiModel),
|
|
//wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free
|
|
wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid
|
|
|
|
|
|
}
|
|
|
|
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
|
|
|
|
const float kappa = 0.75;
|
|
|
|
const MyGridNode& gn = grid->getNodeFor(p.state.position);
|
|
switch (act) {
|
|
|
|
case Activity::STANDING:
|
|
case Activity::WALKING:
|
|
if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;}
|
|
if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;}
|
|
{return 1-kappa;}
|
|
|
|
case Activity::WALKING_UP:
|
|
case Activity::WALKING_DOWN:
|
|
if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;}
|
|
if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
|
|
{return 1-kappa;}
|
|
|
|
}
|
|
|
|
return 1.0;
|
|
|
|
}
|
|
|
|
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 int numAP1 = observation.wifi.entries.size();
|
|
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi);
|
|
const int numAP2 = wifiObs.entries.size();
|
|
|
|
Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2));
|
|
|
|
// sanity check
|
|
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
|
|
|
|
#pragma omp parallel for num_threads(3)
|
|
for (int i = 0; i < Settings::numParticles; ++i) {
|
|
|
|
SMC::Particle<MyState>& p = particles[i];
|
|
|
|
// WiFi free
|
|
//const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, vg.group(observation.wifi));
|
|
|
|
// WiFi grid
|
|
const MyGridNode& node = grid->getNodeFor(p.state.position);
|
|
const double pWiFi = wiFiProbability.getProbability(node, observation.currentTime, wifiObs);
|
|
|
|
|
|
//Log::add("xxx", std::to_string(observation.currentTime.ms()) + "_" + std::to_string(wifiObs.entries[0].ts.ms()));
|
|
|
|
const double pStair = getStairProb(p, observation.activity);
|
|
const double pGPS = 1;
|
|
const double prob = pWiFi * pGPS * pStair;
|
|
|
|
p.weight *= prob; // NOTE: keeps the weight returned by the transition step!
|
|
//p.weight = prob; // does NOT keep the weights returned by the transition step
|
|
if (p.weight != p.weight) {throw Exception("nan");}
|
|
|
|
#pragma omp atomic
|
|
sum += p.weight;
|
|
|
|
}
|
|
|
|
return sum;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
}
|
|
|
|
|
|
|
|
#endif // FILTER_H
|