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

parameter for normal distirbuation approximation are okay

Browse Source
This commit is contained in:
toni
2017-04-19 14:40:51 +02:00
parent fdbd984584
commit afc253aebf
4 changed files with 94 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
code/Settings.h
View File

@@ -14,16 +14,16 @@ namespace Settings {
const int numParticles = 5000;
namespace Mode1 {
const double modeProbability = 0.5;
const double modeProbability = 0.99;
}
namespace Mode2 {
const double modeProbability = 0.5;
const double modeProbability = 0.01;
}
namespace Mixing {
//Eigen::Matrix2d transitionProbabilityMatrix(1,0,0,1);
const double lambda = 0.01;
const double lambda = 0.03;
}
namespace IMU {

33
code/filter/KLB.h
View File

@@ -23,6 +23,7 @@
#include <Indoor/sensors/imu/MotionDetection.h>
#include <Indoor/sensors/pressure/RelativePressure.h>
#include <Indoor/sensors/radio/WiFiGridEstimator.h>
#include <Indoor/sensors/radio/WiFiQualityAnalyzer.h>
#include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
#include <Indoor/math/MovingAVG.h>
@@ -55,6 +56,7 @@
#include "../Settings.h"
double __KLD = 0.0;
double __QUALITY = 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 :( :( :(
@@ -67,7 +69,7 @@ struct ModeProbabilityTransition : public K::MarkovTransitionProbability<MyState
ModeProbabilityTransition(Grid<MyNode>& grid, double lambda) : grid(grid), lambda(lambda) {;}
virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes) override {
virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes, const MyObs& obs) override {
std::vector<double> probsWifiV;
std::vector<double> probsParticleV;
@@ -121,6 +123,7 @@ struct ModeProbabilityTransition : public K::MarkovTransitionProbability<MyState
struct ModeProbabilityTransitionNormal : public K::MarkovTransitionProbability<MyState, MyControl, MyObs>{
const double lambda;
WiFiQualityAnalyzer analyzer;
//this is a hack! it is possible that the sigma of z is getting 0 and therefore the rank decreases to 2 and
//no inverse matrix is possible
@@ -129,7 +132,7 @@ struct ModeProbabilityTransitionNormal : public K::MarkovTransitionProbability<M
/** ctor */
ModeProbabilityTransitionNormal(double lambda) : lambda(lambda) {;}
virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes) override {
virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes, const MyObs& obs) override {
Assert::equal(modes[0].getParticles().size(), modes[1].getParticles().size(), "Particle.size() differs!");
@@ -159,25 +162,37 @@ struct ModeProbabilityTransitionNormal : public K::MarkovTransitionProbability<M
meanWifi << estWifi.x, estWifi.y, estWifi.z;
Distribution::NormalDistributionN normWifi = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(mWifi, meanWifi);
// get kld
double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);
if(kld > 20){
std::cout << "STTTTTOOOOOOP" << std::endl;
//calc wi-fi metrik
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(obs.wifi);
if(!wifiObs.entries.empty()){
analyzer.add(wifiObs);
}
float qualityWifi = analyzer.getQuality();
if(std::isnan(qualityWifi)){
qualityWifi = 1.0;
} else if(qualityWifi == 0) {
qualityWifi = 0.00000001;
}
// debugging global variable
__QUALITY = qualityWifi;
// get kld
double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);
// debugging global variable
__KLD = kld;
//exp. distribution
double expKld = std::exp(-lambda * kld);
double expKld = std::exp(-lambda * (kld * qualityWifi));
Assert::isTrue(expKld < 1.0, "exp. distribution greater 1!");
//create the matrix
Eigen::MatrixXd m(2,2);
m << expKld, 1- expKld, 0, 1;
m << expKld, 1.0 - expKld, 1 - qualityWifi, qualityWifi;
return m;

78
code/filter/Logic.h
View File

@@ -100,9 +100,10 @@ struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
Grid<MyNode>& grid;
GridPoint startPos;
float headingDeg;
int mode;
PFInitFixed(Grid<MyNode>& grid, GridPoint startPos, float headingDeg) :
grid(grid), startPos(startPos), headingDeg(headingDeg) {;}
PFInitFixed(Grid<MyNode>& grid, GridPoint startPos, float headingDeg, int mode) :
grid(grid), startPos(startPos), headingDeg(headingDeg), mode(mode) {;}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
@@ -118,6 +119,9 @@ struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
p.state.heading.error = 0;
p.state.relativePressure = 0; // start with a relative pressure of 0
p.weight = 1.0 / particles.size(); // equal weight
//for debugging
p.state.curMode = mode;
}
}
@@ -130,7 +134,7 @@ struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
// define the noise
Distribution::Normal<float> noise_cm = Distribution::Normal<float>(0.0, Settings::IMU::stepLength * 2.0 * 100.0);
Distribution::Normal<float> height = Distribution::Normal<float>(0.0, 600.0);
Distribution::Normal<float> height_m = Distribution::Normal<float>(0.0, 6.0);
// draw randomly from a vector
random_selector<> rand;
@@ -149,55 +153,35 @@ struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
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);
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;
}
// 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
// }
// }
double diffHeight = p.state.position.z_cm + height.draw();
if()
GridPoint noisePt(noise_cm.draw(), noise_cm.draw(), height.draw());
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);
// p.state.position = grid.getNearestNode(newPosition);
// if(grid.hasNodeFor(newPosition)){
// p.state.position = newPosition;
// }else{
// //no new position!
// #pragma omp atomic
// noNewPositionCounter++;
// }
if(grid.hasNodeFor(newPosition)){
p.state.position = newPosition;
}else{
//no new position!
#pragma omp atomic
noNewPositionCounter++;
}
}
// std::cout << noNewPositionCounter << std::endl;
std::cout << noNewPositionCounter << std::endl;
}
};
@@ -339,7 +323,7 @@ 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);
@@ -347,7 +331,7 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
_assertNotNAN(pWifi, "Wifi prob is nan");
//_assertNot0(pBaroPressure,"pBaroPressure is null");
const double prob = pWifi;
const double prob = pWifi * pBaroPressure;
p.weight = prob;

42
code/main.cpp
View File

@@ -107,6 +107,7 @@ Floorplan::IndoorMap* MyState::map;
void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPath) {
std::vector<double> kld_data;
std::vector<double> quality_data;
// load the floorplan
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
@@ -169,7 +170,9 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
//std::shared_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f));
// mode 1
std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode1(new PFInit(grid, 1));
//std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode1(new PFInit(grid, 1));
std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode1(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f, 1));
K::ParticleFilterMixing<MyState, MyControl, MyObs> mode1(Settings::numParticles, initMode1, Settings::Mode1::modeProbability);
mode1.setTransition(std::shared_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
mode1.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
@@ -180,7 +183,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
modes.push_back(mode1);
// mode 2
std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode2(new PFInit(grid, 2));
//std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode2(new PFInit(grid, 2));
std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode2(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f, 2));
K::ParticleFilterMixing<MyState, MyControl, MyObs> mode2(Settings::numParticles, initMode2, Settings::Mode2::modeProbability);
mode2.setTransition(std::shared_ptr<PFTransSimple>(new PFTransSimple(grid)));
mode2.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
@@ -281,6 +285,7 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
} else{
kld_data.push_back(__KLD);
}
quality_data.push_back(__QUALITY);
Point3 estPos = est.position.inMeter();
@@ -313,6 +318,7 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
plot.gp << "set label 1003 at screen 0.02, 0.92 'KLD: " << ":" << kld_data.back() << "'\n";
plot.gp << "set label 1004 at screen 0.90, 0.98 'act:" << obs.activity << "'\n";
plot.gp << "set label 1011 at screen 0.90, 0.10 'Wifi Quality:" << __QUALITY << "'\n";
plot.gp << "set label 1005 at screen 0.90, 0.08 'Prob. Mode1:" << IMMAPF.getModes()[0].getModePosteriorProbability() << "'\n";
plot.gp << "set label 1006 at screen 0.90, 0.06 'Prob. Mode2:" << IMMAPF.getModes()[1].getModePosteriorProbability() << "'\n";
@@ -390,7 +396,7 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
//save as screenshot for klb
std::string path = evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t);
gp << "set terminal png size 1280,720\n";
gp << "set output '" << path << "_shennendistance.png'\n";
gp << "set output '" << path << "klddistance.png'\n";
for(int i=0; i < kld_data.size()-1; ++i){
@@ -404,6 +410,28 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
gp.draw(plotkld);
gp.flush();
//draw wifi quality
K::Gnuplot gpQuality;
K::GnuplotPlot plotquality;
K::GnuplotPlotElementLines linesQuality;
//save as screenshot for wifiquality
std::string pathWifi = evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t);
gpQuality << "set terminal png size 1280,720\n";
gpQuality << "set output '" << pathWifi << "wifiquality.png'\n";
for(int i=0; i < quality_data.size()-1; ++i){
K::GnuplotPoint2 p1(i, quality_data[i]);
K::GnuplotPoint2 p2(i+1, quality_data[i+1]);
linesQuality.addSegment(p1, p2);
}
plotquality.add(&linesQuality);
gpQuality.draw(plotquality);
gpQuality.flush();
std::cout << "finished" << std::endl;
sleep(1);
@@ -424,10 +452,12 @@ int main(int argc, char** argv) {
// //run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
//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, 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, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
//}