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

commit before new model is implemented

Browse Source
This commit is contained in:
toni
2018-01-17 13:19:16 +01:00
parent bea81eab62
commit f4c598299f
6 changed files with 152 additions and 241 deletions
Download Patch File Download Diff File Expand all files Collapse all files

199
filter/Logic.h
View File

@@ -26,26 +26,28 @@
#include <Indoor/sensors/activity/ActivityDetector.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 <Indoor/smc/filtering/ParticleFilterMixing.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingPercent.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKLD.h>
#include <KLib/math/filter/smoothing/BackwardFilterTransition.h>
#include <Indoor/smc/smoothing/BackwardFilterTransition.h>
#include <Indoor/smc/smoothing/BackwardSimulation.h>
#include <Indoor/smc/sampling/CumulativeSampler.h>
#include "Structs.h"
#include <omp.h>
#include "../Settings.h"
/** particle-filter init randomly distributed within the building*/
struct PFInit : public K::ParticleFilterInitializer<MyState> {
struct PFInit : public SMC::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) {
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
for (SMC::Particle<MyState>& p : particles) {
int idx = rand() % grid.getNumNodes();
p.state.position = grid[idx]; // random position
@@ -60,7 +62,7 @@ struct PFInit : public K::ParticleFilterInitializer<MyState> {
};
/** particle-filter init with fixed position*/
struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
struct PFInitFixed : public SMC::ParticleFilterInitializer<MyState> {
Grid<MyNode>& grid;
GridPoint startPos;
@@ -69,11 +71,11 @@ struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
PFInitFixed(Grid<MyNode>& grid, GridPoint startPos, float headingDeg) :
grid(grid), startPos(startPos), headingDeg(headingDeg) {;}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
Distribution::Normal<float> norm(0.0f, 1.5f);
for (K::Particle<MyState>& p : particles) {
for (SMC::Particle<MyState>& p : particles) {
GridPoint pos = startPos + GridPoint(norm.draw(),norm.draw(),0.0f);
@@ -89,7 +91,7 @@ struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
};
/** very simple transition model, just scatter normal distributed */
struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
struct PFTransSimple : public SMC::ParticleFilterTransition<MyState, MyControl>{
Grid<MyNode>& grid;
@@ -106,13 +108,13 @@ struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
/** ctor */
PFTransSimple(Grid<MyNode>& grid) : grid(grid) {}
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
virtual void transition(std::vector<SMC::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];
SMC::Particle<MyState>& p = particles[i];
// update the baromter
float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
@@ -151,7 +153,7 @@ struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
};
/** particle-filter transition */
struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
struct PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
Grid<MyNode>& grid;
@@ -169,7 +171,6 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
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);
@@ -177,18 +178,18 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
//walker.addModule(&modSpread); // might help in some situations! keep in mind!
//walker.addModule(&modActivity);
//walker.addModule(&modHeadUgly);
walker.addModule(&modImportance);
//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 {
virtual void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
for (K::Particle<MyState>& p : particles) {
for (SMC::Particle<MyState>& p : particles) {
//this is just for the smoothing transition... quick and dirty
p.state.headingChangeMeasured_rad = control->turnSinceLastTransition_rad;
@@ -215,7 +216,7 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
* particle-filter transition
* Adapting the Sample Size in Particle Filters Through KLD-Sampling - D. Fox
*/
struct PFTransKLDSampling : public K::ParticleFilterTransition<MyState, MyControl> {
struct PFTransKLDSampling : public SMC::ParticleFilterTransition<MyState, MyControl> {
Grid<MyNode>& grid;
@@ -248,8 +249,8 @@ struct PFTransKLDSampling : public K::ParticleFilterTransition<MyState, MyContro
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(&modHead);
walker.addModule(&modHeadMises);
//walker.addModule(&modSpread); // might help in some situations! keep in mind!
//walker.addModule(&modActivity);
//walker.addModule(&modHeadUgly);
@@ -267,7 +268,7 @@ struct PFTransKLDSampling : public K::ParticleFilterTransition<MyState, MyContro
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 {
virtual void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
Distribution::Uniform<int> getParticle(0, particles.size()-1);
@@ -281,13 +282,13 @@ struct PFTransKLDSampling : public K::ParticleFilterTransition<MyState, MyContro
bins.clearUsed();
//create new particle set
std::vector<K::Particle<MyState>> particlesNew;
std::vector<SMC::Particle<MyState>> particlesNew;
do{
//draw equally from the particle set
int particleIdx = getParticle.draw();
K::Particle<MyState>& p = particles[particleIdx];
SMC::Particle<MyState>& p = particles[particleIdx];
//sample new particles based on the transition step
// save old position
@@ -310,7 +311,7 @@ struct PFTransKLDSampling : public K::ParticleFilterTransition<MyState, MyContro
bins.markUsed(p.state);
//calculate the new N
double z_delta = K::NormalDistributionCDF::getProbit(1 - delta);
double z_delta = Distribution::NormalCDF<double>::getProbit(1 - delta);
double front = (k - 1) / (2 * epsilon);
double back = 1 - (2 / (9 * (k - 1))) + (std::sqrt(2 / (9 * (k - 1))) * z_delta );
@@ -332,8 +333,7 @@ struct PFTransKLDSampling : public K::ParticleFilterTransition<MyState, MyContro
};
struct BFTrans : public K::BackwardFilterTransition<MyState>{
struct BFTrans : public SMC::BackwardFilterTransition<MyState, MyControl>{
public:
@@ -362,28 +362,21 @@ public:
}
}
std::vector<SMC::Particle<MyState>> transition(std::vector<SMC::Particle<MyState>> const& toBeSmoothedParticles_qt, std::vector<MyControl> const& controls_1T) override{
Assert::doThrow( "Wrong transition function. Use the other one!");
std::vector<SMC::Particle<MyState>> dummyReturn;
return dummyReturn;
}
/**
* smoothing transition starting at T with t, t-1,...0
* @param particles_qt q_t (Forward Filter) p2
* @param particles_qt1 q_t+1 (Smoothed Particles from Step before) p1
*/
std::vector<std::vector<double>> transition(std::vector<K::Particle<MyState>>const& particles_qt,
std::vector<K::Particle<MyState>>const& particles_qt1,
const MyControl* control) override {
// Forward Transition von q_t nach q_t+1* with tracking of particle using an id = p(q_t+1* | q_t)
//TODO: darf ich das einfach? einfach eine neue Dichte Sampeln? Brauch ich da nicht eine "Erlaubnis" (Two-Filter Smoother kann das)
// law of total probabality auch einfach über ziehen ??
// KDE auf q_t+1 Samples = p(q_t+1 | o_1:T)
// Apply Position from Samples from q_t+1* into KDE of p(q_t+1 | o_1:T) to get p(q_t+1* | o_1:T)
// Calculate new weight w(q_(t|T)) = w(q_t) * p(q_t+1* | o_1:T) * p(q_t+1* | q_t) * normalisation
std::vector<std::vector<double>> transition(std::vector<SMC::Particle<MyState>>const& particles_qt,
std::vector<SMC::Particle<MyState>>const& particles_qt1) override {
// calculate alpha(m,n) = p(q_t+1(m) | q_t(n))
@@ -395,33 +388,33 @@ public:
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) {
for (int i = 0; i < particles_qt1.size(); ++i) {
std::vector<double> innerVector;
auto p1 = &particles_old[i];
auto p1 = &particles_qt1[i];
for(int j = 0; j < particles_new.size(); ++j){
for(int j = 0; j < particles_qt.size(); ++j){
auto p2 = &particles_new[j];
auto p2 = &particles_qt[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);
const double distProb = Distribution::Normal<double>::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);
const double headingProb = Distribution::Normal<double>::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);
const double floorProb = Distribution::Normal<double>::getProbability(Settings::Smoothing::zChange, Settings::Smoothing::zSigma, diffZ);
//combine the probabilities
double prob = distProb;// * floorProb * headingProb;
@@ -437,99 +430,7 @@ public:
}
};
struct BFTransKDESlow : 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> {
struct PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObs> {
WiFiModel& wifiModel;
WiFiObserverFree wiFiProbability; // free-calculation
@@ -579,7 +480,7 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
return Distribution::Normal<double>::getProbability(static_cast<double>(hPa), 0.10, static_cast<double>(observation.relativePressure));
}
double getStairProb(const K::Particle<MyState>& p, const Activity act) {
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
const float kappa = 0.65;
@@ -603,7 +504,7 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
}
virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObs& observation) override {
virtual double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObs& observation) override {
double sum = 0;
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
@@ -613,7 +514,7 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
K::Particle<MyState>& p = particles[i];
SMC::Particle<MyState>& p = particles[i];
Point3 pos_m = p.state.position.inMeter();
Point3 posOld_m = p.state.positionOld.inMeter();
@@ -625,8 +526,8 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
//const double pBeacon = getBEACON(observation, p.state.position);
//small checks
_assertNotNAN(pWifi, "Wifi prob is nan");
_assertNot0(pBaroPressure,"pBaroPressure is null");
Assert::isNotNaN(pWifi, "Wifi prob is nan");
Assert::isNot0(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));