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

added the mixing particle filter model with all is might and failures :)

Browse Source
This commit is contained in:
toni
2017-04-16 01:11:59 +02:00
parent c7691a81f0
commit 1f6df67010
6 changed files with 239 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
code/filter/KLB.h
View File

@@ -32,8 +32,7 @@
#include <Indoor/data/Timestamp.h>
#include <KLib/math/filter/particles/Particle.h>
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/ParticleFilterHistory.h>
#include <KLib/math/filter/particles/ParticleFilterMixing.h>
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
@@ -45,12 +44,81 @@
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingDivergence.h>
#include <KLib/math/filter/merging/MarkovTransitionProbability.h>
#include <KLib/math/filter/merging/mixing/MixingSamplerDivergency.h>
#include <KLib/math/filter/merging/estimation/JointEstimationPosteriorOnly.h>
#include "Structs.h"
#include "../Plotti.h"
#include "Logic.h"
#include "../Settings.h"
double __KLD = 0.0;
//todo function return the transition prob matrix for markov chain!
//getKernelDensityProbability should work fine for first shot! nevertheless we need to do 2 kernel density estimations for both filters :( :( :(
struct ModeProbabilityTransition : public K::MarkovTransitionProbability<MyState, MyControl, MyObs>{
Grid<MyNode>& grid;
const double lambda;
ModeProbabilityTransition(Grid<MyNode>& grid, double lambda) : grid(grid), lambda(lambda) {;}
virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes) override {
std::vector<double> probsWifiV;
std::vector<double> probsParticleV;
// mode[0] -> Posterior & mode[1] -> Wifi ---- i know what im doing :)
for(MyNode node : grid.getNodes()){
double probParzenPosterior = calcKernelDensity(node, modes[0].getParticles());
probsParticleV.push_back(probParzenPosterior);
double probParzenWifi = calcKernelDensity(node, modes[1].getParticles());
probsWifiV.push_back(probParzenWifi);
}
// make vectors
Eigen::Map<Eigen::VectorXd> probsWifi(&probsWifiV[0], probsWifiV.size());
Eigen::Map<Eigen::VectorXd> probsParticle(&probsParticleV[0], probsParticleV.size());
// get kld
double kld = Divergence::KullbackLeibler<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
// debugging global variable
__KLD = kld;
//exp. distribution
double expKld = std::exp(-lambda * kld);
//create the matrix
Eigen::MatrixXd m(2,2);
m << 1-expKld, expKld, 0, 1;
return m;
}
double calcKernelDensity(const MyNode node, const std::vector<K::Particle<MyState>> particles){
int size = particles.size();
double prob = 0;
#pragma omp parallel for reduction(+:prob) num_threads(6)
for(int i = 0; i < size; ++i){
double distance = particles[i].state.position.getDistanceInCM(node);
prob += Distribution::Normal<double>::getProbability(0, 100, distance) * particles[i].weight;
}
return prob;
}
};
static double getKernelDensityProbability(std::vector<K::Particle<MyState>>& particles, MyState state, std::vector<K::Particle<MyState>>& samplesWifi){
Distribution::KernelDensity<double, MyState> parzen([&](MyState state){
@@ -92,8 +160,8 @@ static double getKernelDensityProbability(std::vector<K::Particle<MyState>>& par
Eigen::Map<Eigen::VectorXd> probsParticle(&probsParticleV[0], probsParticleV.size());
//get divergence
//double kld = Divergence::KullbackLeibler<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
double kld = Divergence::JensenShannon<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
double kld = Divergence::KullbackLeibler<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
//double kld = Divergence::JensenShannon<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
//plotti
//plot.debugDistribution1(samplesWifi);
@@ -101,9 +169,9 @@ static double getKernelDensityProbability(std::vector<K::Particle<MyState>>& par
//estimate the mean
//K::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
//const MyState estWifi = estimateWifi.estimate(samplesWifi);
//plot.addEstimationNodeSmoothed(estWifi.position.inMeter());
// K::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
// const MyState estWifi = estimateWifi.estimate(samplesWifi);
// plot.addEstimationNodeSmoothed(estWifi.position.inMeter());
return kld;
}

105
code/filter/Logic.h
View File

@@ -23,7 +23,9 @@
#include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
#include <Indoor/sensors/beacon/BeaconProbabilityFree.h>
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <Indoor/math/distribution/Uniform.h>
#include <KLib/math/filter/particles/ParticleFilterMixing.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
@@ -32,6 +34,40 @@
#include "../Settings.h"
#include <random>
#include <iterator>
template <typename RandomGenerator = std::default_random_engine>
struct random_selector
{
//On most platforms, you probably want to use std::random_device("/dev/urandom")()
random_selector(RandomGenerator g = RandomGenerator(std::random_device()()))
: gen(g) {}
template <typename Iter>
Iter select(Iter start, Iter end) {
std::uniform_int_distribution<> dis(0, std::distance(start, end) - 1);
std::advance(start, dis(gen));
return start;
}
//convenience function
template <typename Iter>
Iter operator()(Iter start, Iter end) {
return select(start, end);
}
//convenience function that works on anything with a sensible begin() and end(), and returns with a ref to the value type
template <typename Container>
auto operator()(const Container& c) -> decltype(*begin(c))& {
return *select(begin(c), end(c));
}
private:
RandomGenerator gen;
};
/** particle-filter init randomly distributed within the building*/
struct PFInit : public K::ParticleFilterInitializer<MyState> {
@@ -83,6 +119,63 @@ struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
};
/** very simple transition model, just scatter normal distributed */
struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
Grid<MyNode>& grid;
std::minstd_rand gen;
random_selector<> rand;
Distribution::Uniform<float> uniRand = Distribution::Uniform<float>(0,1);
PFTransSimple(Grid<MyNode>& grid) : grid(grid) {}
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
std::normal_distribution<float> noise_cm(0.0, Settings::IMU::stepLength * 2.0 * 100.0);
#pragma omp parallel for num_threads(6)
for (int i = 0; i < Settings::numParticles; ++i) {
K::Particle<MyState>& p = particles[i];
// if neighboring node is a staircase, we have a 0.8 chance to walk them.
GridPoint tmp = grid.getNodeFor(p.state.position);
MyNode tmpNode(tmp);
int numNeigbors = grid.getNumNeighbors(tmpNode);
std::vector<MyNode> zNodes;
for(int i = 0; i < numNeigbors; ++i){
//if neighbor is stair (1) or elevator (2)
MyNode curNode = grid.getNeighbor(tmpNode, i);
if(curNode.getType() == 1 || curNode.getType() == 2){
zNodes.push_back(curNode);
}
}
float height = 0.0;
if(!zNodes.empty()){
if(uniRand.draw() > 0.3){
//get a random height from all the neighbors on stairs or elevators
height = rand(zNodes).z_cm - p.state.position.z_cm;
}else{
//do nothin
}
}
GridPoint noisePt(noise_cm(gen), noise_cm(gen), height);
GridPoint newPosition = p.state.position + noisePt;
if(grid.hasNodeFor(newPosition)){
p.state.position = newPosition;
}else{
//no new position!
}
}
}
};
/** particle-filter transition */
struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
@@ -165,8 +258,8 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
/** 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);
//const MyNode& node = grid.getNodeFor(point);
return wiFiProbability.getProbability(point.inMeter(), observation.currentTime, vapWifi);
}
/** probability for BEACONS */
@@ -219,15 +312,15 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
// Point3 posOld_m = p.state.positionOld.inMeter();
const double pWifi = getWIFI(observation, wifiObs, p.state.position);
const double pBaroPressure = getStairProb(p, observation.activity);
//const double pBaroPressure = getStairProb(p, observation.activity);
//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");
//_assertNot0(pBaroPressure,"pBaroPressure is null");
const double prob = pBaroPressure * pWifi;
const double prob = pWifi;
p.weight = prob;

2
code/filter/Structs.h
View File

@@ -108,6 +108,8 @@ struct MyNode : public GridPoint, public GridNode, public GridNodeImportance, pu
/** ctor */
MyNode(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
MyNode(const GridPoint point) : GridPoint(point.x_cm, point.y_cm, point.z_cm) {;}
static void staticDeserialize(std::istream& inp) {
WiFiGridNode::staticDeserialize(inp);
}