first commit init project
This commit is contained in:
toni
554
filter/Logic.h
Normal file
554
filter/Logic.h
Normal file
@@ -0,0 +1,554 @@
|
||||
#ifndef FLOGIC_H
|
||||
#define FLOGIC_H
|
||||
|
||||
#include <Indoor/grid/Grid.h>
|
||||
|
||||
#include <Indoor/grid/walk/v2/GridWalker.h>
|
||||
#include <Indoor/grid/walk/v2/GridWalkerMulti.h>
|
||||
|
||||
#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
|
||||
#include <Indoor/grid/walk/v2/modules/WalkModuleHeading.h>
|
||||
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
|
||||
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingVonMises.h>
|
||||
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
|
||||
#include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
|
||||
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
|
||||
#include <Indoor/grid/walk/v2/modules/WalkModulePreventVisited.h>
|
||||
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
|
||||
#include <Indoor/sensors/radio/WiFiQualityAnalyzer.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/BeaconModelLogDistCeiling.h>
|
||||
#include <Indoor/sensors/beacon/BeaconProbabilityFree.h>
|
||||
|
||||
#include <KLib/math/filter/particles/ParticleFilterMixing.h>
|
||||
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
|
||||
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
|
||||
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingKLD.h>
|
||||
|
||||
#include <KLib/math/filter/smoothing/BackwardFilterTransition.h>
|
||||
|
||||
#include "Structs.h"
|
||||
#include <omp.h>
|
||||
#include "../Settings.h"
|
||||
|
||||
/** particle-filter init randomly distributed within the building*/
|
||||
struct PFInit : public K::ParticleFilterInitializer<MyState> {
|
||||
|
||||
Grid<MyNode>& grid;
|
||||
|
||||
PFInit(Grid<MyNode>& grid) : grid(grid) {;}
|
||||
|
||||
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
|
||||
for (K::Particle<MyState>& p : particles) {
|
||||
|
||||
int idx = rand() % grid.getNumNodes();
|
||||
p.state.position = grid[idx]; // random position
|
||||
p.state.heading.direction = (rand() % 360) / 180.0 * M_PI; // random heading
|
||||
p.state.heading.error = 0;
|
||||
p.state.relativePressure = 0; // start with a relative pressure of 0
|
||||
p.weight = 1.0 / particles.size(); // equal weight
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/** particle-filter init with fixed position*/
|
||||
struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
|
||||
|
||||
Grid<MyNode>& grid;
|
||||
GridPoint startPos;
|
||||
float headingDeg;
|
||||
|
||||
PFInitFixed(Grid<MyNode>& grid, GridPoint startPos, float headingDeg) :
|
||||
grid(grid), startPos(startPos), headingDeg(headingDeg) {;}
|
||||
|
||||
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
|
||||
|
||||
Distribution::Normal<float> norm(0.0f, 1.5f);
|
||||
|
||||
for (K::Particle<MyState>& p : particles) {
|
||||
|
||||
GridPoint pos = startPos + GridPoint(norm.draw(),norm.draw(),0.0f);
|
||||
|
||||
GridPoint startPos = grid.getNodeFor(pos);
|
||||
p.state.position = startPos; // scatter arround the start position
|
||||
p.state.heading.direction = headingDeg / 180.0 * M_PI; // fixed heading
|
||||
p.state.heading.error = 0;
|
||||
p.state.relativePressure = 0; // start with a relative pressure of 0
|
||||
p.weight = 1.0 / particles.size(); // equal weight
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/** very simple transition model, just scatter normal distributed */
|
||||
struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
|
||||
|
||||
Grid<MyNode>& grid;
|
||||
|
||||
// define the noise
|
||||
Distribution::Normal<float> noise_cm = Distribution::Normal<float>(0.0, Settings::IMU::stepLength * 2.0 * 100.0);
|
||||
Distribution::Normal<float> height_m = Distribution::Normal<float>(0.0, 6.0);
|
||||
|
||||
// draw randomly from a vector
|
||||
//random_selector<> rand;
|
||||
|
||||
// draw from 0 - 1
|
||||
Distribution::Uniform<float> uniRand = Distribution::Uniform<float>(0,1);
|
||||
|
||||
/** ctor */
|
||||
PFTransSimple(Grid<MyNode>& grid) : grid(grid) {}
|
||||
|
||||
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
|
||||
|
||||
//int noNewPositionCounter = 0;
|
||||
|
||||
#pragma omp parallel for num_threads(6)
|
||||
for (int i = 0; i < particles.size(); ++i) {
|
||||
K::Particle<MyState>& p = particles[i];
|
||||
|
||||
// update the baromter
|
||||
float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
|
||||
p.state.relativePressure += deltaZ_cm * 0.105f;
|
||||
|
||||
double diffHeight = p.state.position.inMeter().z + height_m.draw();
|
||||
double newHeight_cm = p.state.position.z_cm;
|
||||
if(diffHeight > 9.1){
|
||||
newHeight_cm = 10.8 * 100.0;
|
||||
} else if (diffHeight < 9.1 && diffHeight > 5.7){
|
||||
newHeight_cm = 7.4 * 100.0;
|
||||
} else if (diffHeight < 5.7 && diffHeight > 2.0) {
|
||||
newHeight_cm = 4.0 * 100.0;
|
||||
} else {
|
||||
newHeight_cm = 0.0;
|
||||
}
|
||||
|
||||
GridPoint noisePt(noise_cm.draw(), noise_cm.draw(), 0.0);
|
||||
GridPoint newPosition = p.state.position + noisePt;
|
||||
newPosition.z_cm = newHeight_cm;
|
||||
|
||||
// p.state.position = grid.getNearestNode(newPosition);
|
||||
|
||||
if(grid.hasNodeFor(newPosition)){
|
||||
p.state.position = newPosition;
|
||||
}else{
|
||||
//no new position!
|
||||
// #pragma omp atomic
|
||||
// noNewPositionCounter++;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// std::cout << noNewPositionCounter << std::endl;
|
||||
}
|
||||
};
|
||||
|
||||
/** particle-filter transition */
|
||||
struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
|
||||
|
||||
Grid<MyNode>& grid;
|
||||
|
||||
GridWalker<MyNode, MyState> walker;
|
||||
|
||||
WalkModuleHeading<MyNode, MyState> modHeadUgly; // stupid
|
||||
WalkModuleHeadingControl<MyNode, MyState, MyControl> modHead;
|
||||
WalkModuleHeadingVonMises<MyNode, MyState, MyControl> modHeadMises;
|
||||
WalkModuleNodeImportance<MyNode, MyState> modImportance;
|
||||
WalkModuleSpread<MyNode, MyState> modSpread;
|
||||
WalkModuleFavorZ<MyNode, MyState> modFavorZ;
|
||||
//WalkModulePreventVisited<MyNode, MyState> modPreventVisited;
|
||||
|
||||
//WalkModuleActivityControl<MyNode, MyState, MyControl> modActivity;
|
||||
|
||||
std::minstd_rand gen;
|
||||
|
||||
|
||||
PFTrans(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma), modHeadMises(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) {
|
||||
|
||||
walker.addModule(&modHead);
|
||||
//walker.addModule(&modHeadMises);
|
||||
//walker.addModule(&modSpread); // might help in some situations! keep in mind!
|
||||
//walker.addModule(&modActivity);
|
||||
//walker.addModule(&modHeadUgly);
|
||||
walker.addModule(&modImportance);
|
||||
//walker.addModule(&modFavorZ);
|
||||
//walker.addModule(&modButterAct);
|
||||
//walker.addModule(&modWifi);
|
||||
//walker.addModule(&modPreventVisited);
|
||||
}
|
||||
|
||||
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
|
||||
|
||||
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
|
||||
|
||||
for (K::Particle<MyState>& p : particles) {
|
||||
|
||||
//this is just for the smoothing transition... quick and dirty
|
||||
p.state.headingChangeMeasured_rad = control->turnSinceLastTransition_rad;
|
||||
|
||||
// save old position
|
||||
p.state.positionOld = p.state.position; //GridPoint(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
|
||||
|
||||
// update steps
|
||||
const float dist_m = std::abs(control->numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
|
||||
|
||||
// update the particle in-place
|
||||
p.state = walker.getDestination(grid, p.state, dist_m);
|
||||
|
||||
// update the baromter
|
||||
float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
|
||||
p.state.relativePressure += deltaZ_cm * 0.105f;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* particle-filter transition
|
||||
* Adapting the Sample Size in Particle Filters Through KLD-Sampling - D. Fox
|
||||
*/
|
||||
struct PFTransKLDSampling : public K::ParticleFilterTransition<MyState, MyControl> {
|
||||
|
||||
Grid<MyNode>& grid;
|
||||
|
||||
GridWalker<MyNode, MyState> walker;
|
||||
|
||||
WalkModuleHeading<MyNode, MyState> modHeadUgly; // stupid
|
||||
WalkModuleHeadingControl<MyNode, MyState, MyControl> modHead;
|
||||
WalkModuleHeadingVonMises<MyNode, MyState, MyControl> modHeadMises;
|
||||
WalkModuleNodeImportance<MyNode, MyState> modImportance;
|
||||
WalkModuleSpread<MyNode, MyState> modSpread;
|
||||
WalkModuleFavorZ<MyNode, MyState> modFavorZ;
|
||||
//WalkModulePreventVisited<MyNode, MyState> modPreventVisited;
|
||||
|
||||
//WalkModuleActivityControl<MyNode, MyState, MyControl> modActivity;
|
||||
|
||||
std::minstd_rand gen;
|
||||
|
||||
/** upper bound epsilon of the kld distance - the particle size is not allowed to exceed epsilon*/
|
||||
double epsilon;
|
||||
|
||||
/** the upper 1 - delta quantil of the normal distribution. something like 0.01 */
|
||||
double delta;
|
||||
|
||||
/** the bins */
|
||||
Binning<MyState> bins;
|
||||
|
||||
/** max particle size */
|
||||
uint32_t N_max;
|
||||
|
||||
|
||||
PFTransKLDSampling(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma), modHeadMises(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) {
|
||||
|
||||
walker.addModule(&modHead);
|
||||
//walker.addModule(&modHeadMises);
|
||||
//walker.addModule(&modSpread); // might help in some situations! keep in mind!
|
||||
//walker.addModule(&modActivity);
|
||||
//walker.addModule(&modHeadUgly);
|
||||
walker.addModule(&modImportance);
|
||||
//walker.addModule(&modFavorZ);
|
||||
//walker.addModule(&modButterAct);
|
||||
//walker.addModule(&modWifi);
|
||||
//walker.addModule(&modPreventVisited);
|
||||
|
||||
|
||||
epsilon = 0.15;
|
||||
delta = 0.01;
|
||||
N_max = 5000;
|
||||
bins.setBinSizes({0.01, 0.01, 0.2, 0.3});
|
||||
bins.setRanges({BinningRange(-1,100), BinningRange(-10,60), BinningRange(-1,15), BinningRange(0, 2 * M_PI)});
|
||||
}
|
||||
|
||||
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
|
||||
|
||||
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
|
||||
Distribution::Uniform<int> getParticle(0, particles.size()-1);
|
||||
|
||||
//init stuff
|
||||
uint32_t n = 0;
|
||||
uint32_t k = 1;
|
||||
double N = 0;
|
||||
|
||||
//clear the bins
|
||||
bins.clearUsed();
|
||||
|
||||
//create new particle set
|
||||
std::vector<K::Particle<MyState>> particlesNew;
|
||||
|
||||
do{
|
||||
|
||||
//draw equally from the particle set
|
||||
int particleIdx = getParticle.draw();
|
||||
K::Particle<MyState>& p = particles[particleIdx];
|
||||
|
||||
//sample new particles based on the transition step
|
||||
// save old position
|
||||
p.state.positionOld = p.state.position; //GridPoint(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
|
||||
|
||||
// update steps
|
||||
const float dist_m = std::abs(control->numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
|
||||
|
||||
// update the particle in-place
|
||||
p.state = walker.getDestination(grid, p.state, dist_m);
|
||||
|
||||
// update the baromter
|
||||
float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
|
||||
p.state.relativePressure += deltaZ_cm * 0.105f;
|
||||
|
||||
//if it falls into an empty bin then draw another particle
|
||||
//is bin free?
|
||||
if(bins.isFree(p.state)){
|
||||
k++;
|
||||
bins.markUsed(p.state);
|
||||
|
||||
//calculate the new N
|
||||
double z_delta = K::NormalDistributionCDF::getProbit(1 - delta);
|
||||
double front = (k - 1) / (2 * epsilon);
|
||||
double back = 1 - (2 / (9 * (k - 1))) + (std::sqrt(2 / (9 * (k - 1))) * z_delta );
|
||||
|
||||
N = front * std::pow(back, 3.0);
|
||||
}
|
||||
++n;
|
||||
|
||||
//add particle to new particleset
|
||||
particlesNew.push_back(p);
|
||||
|
||||
|
||||
} while (n < N && n < N_max);
|
||||
|
||||
|
||||
//write new particleset
|
||||
particles.clear();
|
||||
particles.swap(particlesNew);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
struct BFTrans : public K::BackwardFilterTransition<MyState>{
|
||||
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* ctor
|
||||
* @param choice the choice to use for randomly drawing nodes
|
||||
* @param fp the underlying floorplan
|
||||
*/
|
||||
BFTrans()
|
||||
{
|
||||
//nothin
|
||||
}
|
||||
|
||||
uint64_t ts = 0;
|
||||
uint64_t deltaMS = 0;
|
||||
|
||||
/** set the current time in millisconds */
|
||||
void setCurrentTime(const uint64_t ts) {
|
||||
if (this->ts == 0) {
|
||||
this->ts = ts;
|
||||
deltaMS = 0;
|
||||
} else {
|
||||
deltaMS = this->ts - ts;
|
||||
this->ts = ts;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* smoothing transition starting at T with t, t-1,...0
|
||||
* @param particles_new p_t (Forward Filter) p2
|
||||
* @param particles_old p_t+1 (Smoothed Particles from Step before) p1
|
||||
* q(p1 | p2) is calculated
|
||||
*/
|
||||
std::vector<std::vector<double>> transition(std::vector<K::Particle<MyState>>const& particles_new,
|
||||
std::vector<K::Particle<MyState>>const& particles_old ) override {
|
||||
|
||||
|
||||
// calculate alpha(m,n) = p(q_t+1(m) | q_t(n))
|
||||
// this means, predict all possible states q_t+1 with all passible states q_t
|
||||
// e.g. p(q_490(1)|q_489(1));p(q_490(1)|q_489(2)) ... p(q_490(1)|q_489(N)) and
|
||||
// p(q_490(1)|q_489(1)); p(q_490(2)|q_489(1)) ... p(q_490(M)|q_489(1))
|
||||
std::vector<std::vector<double>> predictionProbabilities;
|
||||
|
||||
omp_set_dynamic(0); // Explicitly disable dynamic teams
|
||||
omp_set_num_threads(7);
|
||||
#pragma omp parallel for shared(predictionProbabilities)
|
||||
for (int i = 0; i < particles_old.size(); ++i) {
|
||||
std::vector<double> innerVector;
|
||||
auto p1 = &particles_old[i];
|
||||
|
||||
for(int j = 0; j < particles_new.size(); ++j){
|
||||
|
||||
auto p2 = &particles_new[j];
|
||||
|
||||
const double distance_m = p2->state.position.inMeter().getDistance(p1->state.position.inMeter()) / 100.0;
|
||||
|
||||
//TODO Incorporated Activity - see IPIN16 MySmoothingTransitionExperimental
|
||||
|
||||
const double distProb = K::NormalDistribution::getProbability(Settings::Smoothing::stepLength, Settings::Smoothing::stepSigma, distance_m);
|
||||
|
||||
// TODO: FIX THIS CORRECTLY is the heading change similiar to the measurement?
|
||||
double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.heading.direction.getRAD(), p1->state.heading.direction.getRAD());
|
||||
double diffDeg = Angle::radToDeg(diffRad);
|
||||
double measurementRad = Angle::makeSafe_2PI(p1->state.headingChangeMeasured_rad);
|
||||
double measurementDeg = Angle::radToDeg(measurementRad);
|
||||
const double headingProb = K::NormalDistribution::getProbability(measurementDeg, Settings::Smoothing::headingSigma, diffDeg);
|
||||
|
||||
// does the angle between two particles positions is similiar to the measurement
|
||||
//double angleBetweenParticles = Angle::getDEG_360(p2->state.position.x, p2->state.position.y, p1->state.position.x, p1->state.position.y);
|
||||
|
||||
//check how near we are to the measurement
|
||||
double diffZ = (p2->state.position.inMeter().z - p1->state.position.inMeter().z) / 100.0;
|
||||
const double floorProb = K::NormalDistribution::getProbability(Settings::Smoothing::zChange, Settings::Smoothing::zSigma, diffZ);
|
||||
|
||||
//combine the probabilities
|
||||
double prob = distProb;// * floorProb * headingProb;
|
||||
innerVector.push_back(prob);
|
||||
|
||||
}
|
||||
#pragma omp critical
|
||||
predictionProbabilities.push_back(innerVector);
|
||||
}
|
||||
|
||||
return predictionProbabilities;
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
|
||||
|
||||
WiFiModel& wifiModel;
|
||||
WiFiObserverFree wiFiProbability; // free-calculation
|
||||
//WiFiObserverGrid<MyNode> wiFiProbability; // grid-calculation
|
||||
WiFiQualityAnalyzer wqa;
|
||||
|
||||
BeaconModelLogDistCeiling& beaconModel;
|
||||
BeaconObserverFree beaconProbability;
|
||||
|
||||
Grid<MyNode>& grid;
|
||||
|
||||
PFEval(WiFiModel& wifiModel, BeaconModelLogDistCeiling& beaconModel, Grid<MyNode>& grid) :
|
||||
wifiModel(wifiModel),
|
||||
beaconModel(beaconModel),
|
||||
grid(grid),
|
||||
wiFiProbability(Settings::WiFiModel::sigma, wifiModel),
|
||||
beaconProbability(Settings::BeaconModel::sigma, beaconModel){
|
||||
|
||||
}
|
||||
|
||||
/** probability step-distance */
|
||||
//TODO: add number of recognized steps
|
||||
inline double getStepDistanceProb(const Point3 particle1, const Point3 particle2){
|
||||
double distance = particle1.getDistance(particle2);
|
||||
return Distribution::Normal<double>::getProbability(Settings::IMU::stepLength, Settings::IMU::stepSigma + 0.4, distance);
|
||||
}
|
||||
|
||||
//TODO: combinied evaluation heading and distance
|
||||
|
||||
/** probability for WIFI */
|
||||
inline double getWIFI(const MyObs& observation, const WiFiMeasurements& vapWifi, const GridPoint& point) const {
|
||||
|
||||
const MyNode& node = grid.getNodeFor(point);
|
||||
return wiFiProbability.getProbability(node, observation.currentTime, vapWifi);
|
||||
}
|
||||
|
||||
/** probability for BEACONS */
|
||||
inline double getBEACON(const MyObs& observation, const GridPoint& point){
|
||||
|
||||
//consider adding the persons height
|
||||
Point3 p = point.inMeter() + Point3(0,0,1.3);
|
||||
return beaconProbability.getProbability(p, observation.currentTime, observation.beacons);
|
||||
}
|
||||
|
||||
/** probability for Barometer */
|
||||
inline double getBaroPressure(const MyObs& observation, const float hPa) const{
|
||||
return Distribution::Normal<double>::getProbability(static_cast<double>(hPa), 0.10, static_cast<double>(observation.relativePressure));
|
||||
}
|
||||
|
||||
double getStairProb(const K::Particle<MyState>& p, const ActivityButterPressure::Activity act) {
|
||||
|
||||
const float kappa = 0.75;
|
||||
|
||||
const MyNode& gn = grid.getNodeFor(p.state.position);
|
||||
switch (act) {
|
||||
|
||||
case ActivityButterPressure::Activity::STAY:
|
||||
if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;}
|
||||
if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;}
|
||||
{return 1-kappa;}
|
||||
|
||||
case ActivityButterPressure::Activity::UP:
|
||||
case ActivityButterPressure::Activity::DOWN:
|
||||
if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;}
|
||||
if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
|
||||
{return 1-kappa;}
|
||||
|
||||
}
|
||||
|
||||
return 1.0;
|
||||
|
||||
}
|
||||
|
||||
virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObs& observation) override {
|
||||
|
||||
double sum = 0;
|
||||
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
|
||||
|
||||
wqa.add(wifiObs);
|
||||
float quality = wqa.getQuality();
|
||||
|
||||
#pragma omp parallel for num_threads(3)
|
||||
for (int i = 0; i < particles.size(); ++i) {
|
||||
K::Particle<MyState>& p = particles[i];
|
||||
|
||||
Point3 pos_m = p.state.position.inMeter();
|
||||
Point3 posOld_m = p.state.positionOld.inMeter();
|
||||
|
||||
double pWifi = getWIFI(observation, wifiObs, p.state.position);
|
||||
const double pBaroPressure = getStairProb(p, observation.activity);
|
||||
const double pStepDistance = getStepDistanceProb(pos_m, posOld_m);
|
||||
//const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure);
|
||||
//const double pBeacon = getBEACON(observation, p.state.position);
|
||||
|
||||
//small checks
|
||||
_assertNotNAN(pWifi, "Wifi prob is nan");
|
||||
_assertNot0(pBaroPressure,"pBaroPressure is null");
|
||||
|
||||
const bool volatile init = observation.currentTime.sec() < 25;
|
||||
//double pWiFiMod = (init) ? (std::pow(pWiFi, 0.1)) : (std::pow(pWiFi, 0.5));
|
||||
//double pWiFiMod = (init) ? (std::pow(pWifi, 0.5)) : (std::pow(pWifi, 0.9));
|
||||
|
||||
// bad wifi? -> we have no idea where we are!
|
||||
if (quality < 0.25 && !init) {
|
||||
//pWifi = 1;
|
||||
//p.weight = std::pow(p.weight, 0.5);
|
||||
}
|
||||
|
||||
const double prob = pWifi * pStepDistance;// * pBaroPressure;
|
||||
|
||||
p.weight = prob;
|
||||
|
||||
#pragma omp atomic
|
||||
sum += (prob);
|
||||
|
||||
}
|
||||
|
||||
if(sum == 0.0){
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
return sum;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // FLOGIC_H
|
||||
Reference in New Issue
Block a user