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

2
code/CMakeLists.txt
View File

@@ -58,7 +58,7 @@ ADD_DEFINITIONS(
-fstack-protector-all
-g3
#-O2
-O2
-march=native
-DWITH_TESTS

15
code/Settings.h
View File

@@ -5,12 +5,27 @@
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/VAPGrouper.h>
#include <eigen3/Eigen/Dense>
namespace Settings {
bool useKLB = true;
const int numParticles = 5000;
namespace Mode1 {
const double modeProbability = 0.5;
}
namespace Mode2 {
const double modeProbability = 0.5;
}
namespace Mixing {
//Eigen::Matrix2d transitionProbabilityMatrix(1,0,0,1);
const double lambda = 0.05;
}
namespace IMU {
const float turnSigma = 2.5; // 3.5
const float stepLength = 1.00;

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);
}

107
code/main.cpp
View File

@@ -9,6 +9,7 @@
#include <sys/types.h>
#include <sys/stat.h>
#include <KLib/math/filter/merging/InteractingMultipleModelParticleFilter.h>
//frank
//const std::string mapDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/maps/";
@@ -162,32 +163,41 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
ctrl.resetAfterTransition();
MyObs obs;
//random start position
std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid)); std::move(init);
//filter init
//std::unique_ptr<PFInit> init =
K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)));
//K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInitFixed>(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f)));
pf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
pf.setEvaluation(std::unique_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
//init the mode filters
std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>> modes;
//resampling
if(Settings::useKLB){
pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingDivergence<MyState>>(new K::ParticleFilterResamplingDivergence<MyState>()));
} else {
//pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.4)));
//pf.setResampling(std::unique_ptr<NodeResampling<MyState, MyNode>>(new NodeResampling<MyState, MyNode>(*grid)););
}
std::shared_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
//std::shared_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f));
pf.setNEffThreshold(0.95);
// mode 1
K::ParticleFilterMixing<MyState, MyControl, MyObs> mode1(Settings::numParticles, init, Settings::Mode1::modeProbability);
mode1.setTransition(std::shared_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
mode1.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
mode1.setResampling(std::shared_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
mode1.setNEffThreshold(0.95);
mode1.setEstimator(std::shared_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//estimation
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5)));
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationKernelDensity<MyState, 3>>(new K::ParticleFilterEstimationKernelDensity<MyState, 3>()));
modes.push_back(mode1);
// mode 2
K::ParticleFilterMixing<MyState, MyControl, MyObs> mode2(Settings::numParticles, init, Settings::Mode2::modeProbability);
mode2.setTransition(std::shared_ptr<PFTransSimple>(new PFTransSimple(grid)));
mode2.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
mode2.setResampling(std::shared_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
mode2.setNEffThreshold(0.95);
mode2.setEstimator(std::shared_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
modes.push_back(mode2);
//init mixing
Eigen::MatrixXd transitionProbabilityMatrix(2,2);
transitionProbabilityMatrix << 1,0,0,1;
K::InteractingMultipleModelParticleFilter<MyState, MyControl, MyObs> IMMAPF(modes, transitionProbabilityMatrix);
IMMAPF.setMixingSampler(std::unique_ptr<K::MixingSamplerDivergency<MyState, MyControl, MyObs>>(new K::MixingSamplerDivergency<MyState, MyControl, MyObs>()));
IMMAPF.setJointEstimation(std::unique_ptr<K::JointEstimationPosteriorOnly<MyState, MyControl, MyObs>>(new K::JointEstimationPosteriorOnly<MyState, MyControl, MyObs>()));
IMMAPF.setMarkovTransitionProbability(std::unique_ptr<ModeProbabilityTransition>(new ModeProbabilityTransition(grid, Settings::Mixing::lambda)));
Timestamp lastTimestamp = Timestamp::fromMS(0);
@@ -201,9 +211,6 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
K::Statistics<float> errorStats;
//calc wi-fi prob for every node and get mean vector
WiFiObserverFree wiFiProbability(Settings::WiFiModel::sigma, WiFiModel);
//file writing for error data
const long int t = static_cast<long int>(time(NULL));
@@ -256,7 +263,6 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
} else if (e.type == Offline::Sensor::GRAVITY) {
md.addGravity(ts, fr.getGravity()[e.idx].data);
Eigen::Vector2f curVec = md.getCurrentMotionAxis();
ctrl.motionDeltaAngle_rad = md.getMotionChangeInRad();
}
@@ -265,34 +271,14 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
obs.currentTime = ts;
MyState est;
if(Settings::useKLB){
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(obs.wifi);
//update filter
est = IMMAPF.update(&ctrl, obs);
std::vector<MyNode> allNodes = grid.getNodes();
std::vector<K::Particle<MyState>> particleWifi;
for(MyNode node : allNodes){
double prob = wiFiProbability.getProbability(node, ts, wifiObs);
K::Particle<MyState> tmp (MyState(GridPoint(node.x_cm, node.y_cm, node.z_cm)), prob);
particleWifi.push_back(tmp);
}
if(kld_data.empty()){
kld_data.push_back(0.0);
}
double kld = 0.0;
//set probability distributions.
//std::function<double(std::vector<K::Particle<MyState>>&, MyState, std::vector<K::Particle<MyState>>&)> kldFunc = getKernelDensityProbability;
std::function<double(std::vector<K::Particle<MyState>>&, MyState, std::vector<K::Particle<MyState>>&)> kldFunc = kldFromMultivariatNormal;
//update filter
est = pf.update(&ctrl, obs, particleWifi, kldFunc, kld);
kld_data.push_back(kld);
} else {
est = pf.update(&ctrl, obs);
if(kld_data.empty()){
kld_data.push_back(0.0);
} else{
kld_data.push_back(__KLD);
}
Point3 estPos = est.position.inMeter();
@@ -314,7 +300,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
plot.setEst(estPos);
plot.setGT(gtPos);
plot.addEstimationNode(estPos);
plot.addParticles(pf.getParticles());
plot.addParticles(IMMAPF.getModes()[0].getParticles());
plot.addEstimationNodeSmoothed(IMMAPF.getModes()[1].getEstimation().position.inMeter());
//plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
@@ -404,13 +391,13 @@ int main(int argc, char** argv) {
//run(data.BERKWERK, 6, "EVALBERGWERK"); // Nexus vor
//for(int i = 0; i < 5; ++i){
Settings::useKLB = false;
//run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1);
run(data.IPIN2017, 1, "ipin2017", Settings::Paths_IPIN2017::path1);
run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2);
//run(data.IPIN2017, 3, "ipin2017", Settings::Paths_IPIN2017::path2);
run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3);
//run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
// Settings::useKLB = false;
// //run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1);
// run(data.IPIN2017, 1, "ipin2017", Settings::Paths_IPIN2017::path1);
// run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2);
// //run(data.IPIN2017, 3, "ipin2017", Settings::Paths_IPIN2017::path2);
// run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3);
// //run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
Settings::useKLB = true;
//run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1);