diff --git a/code/Settings.h b/code/Settings.h index df81cf4..5de47b0 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -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 { diff --git a/code/filter/KLB.h b/code/filter/KLB.h index 2747282..c5362a7 100644 --- a/code/filter/KLB.h +++ b/code/filter/KLB.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -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& grid, double lambda) : grid(grid), lambda(lambda) {;} - virtual Eigen::MatrixXd update(std::vector>& modes) override { + virtual Eigen::MatrixXd update(std::vector>& modes, const MyObs& obs) override { std::vector probsWifiV; std::vector probsParticleV; @@ -121,6 +123,7 @@ struct ModeProbabilityTransition : public K::MarkovTransitionProbability{ 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>& modes) override { + virtual Eigen::MatrixXd update(std::vector>& 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::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::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; diff --git a/code/filter/Logic.h b/code/filter/Logic.h index 2132e15..5d2daee 100644 --- a/code/filter/Logic.h +++ b/code/filter/Logic.h @@ -100,9 +100,10 @@ struct PFInitFixed : public K::ParticleFilterInitializer { Grid& grid; GridPoint startPos; float headingDeg; + int mode; - PFInitFixed(Grid& grid, GridPoint startPos, float headingDeg) : - grid(grid), startPos(startPos), headingDeg(headingDeg) {;} + PFInitFixed(Grid& grid, GridPoint startPos, float headingDeg, int mode) : + grid(grid), startPos(startPos), headingDeg(headingDeg), mode(mode) {;} virtual void initialize(std::vector>& particles) override { @@ -118,6 +119,9 @@ struct PFInitFixed : public K::ParticleFilterInitializer { 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{ // define the noise Distribution::Normal noise_cm = Distribution::Normal(0.0, Settings::IMU::stepLength * 2.0 * 100.0); - Distribution::Normal height = Distribution::Normal(0.0, 600.0); + Distribution::Normal height_m = Distribution::Normal(0.0, 6.0); // draw randomly from a vector random_selector<> rand; @@ -149,55 +153,35 @@ struct PFTransSimple : public K::ParticleFilterTransition{ for (int i = 0; i < Settings::numParticles; ++i) { K::Particle& 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 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 { // 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 { _assertNotNAN(pWifi, "Wifi prob is nan"); //_assertNot0(pBaroPressure,"pBaroPressure is null"); - const double prob = pWifi; + const double prob = pWifi * pBaroPressure; p.weight = prob; diff --git a/code/main.cpp b/code/main.cpp index 894b5a9..c111bee 100755 --- a/code/main.cpp +++ b/code/main.cpp @@ -107,6 +107,7 @@ Floorplan::IndoorMap* MyState::map; void run(DataSetup setup, int numFile, std::string folder, std::vector gtPath) { std::vector kld_data; + std::vector 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 gtPa //std::shared_ptr> init(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f)); // mode 1 - std::shared_ptr> initMode1(new PFInit(grid, 1)); + //std::shared_ptr> initMode1(new PFInit(grid, 1)); + std::shared_ptr> initMode1(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f, 1)); + K::ParticleFilterMixing mode1(Settings::numParticles, initMode1, Settings::Mode1::modeProbability); mode1.setTransition(std::shared_ptr(new PFTrans(grid, &ctrl))); mode1.setEvaluation(std::shared_ptr(new PFEval(WiFiModel, beaconModel, grid))); @@ -180,7 +183,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa modes.push_back(mode1); // mode 2 - std::shared_ptr> initMode2(new PFInit(grid, 2)); + //std::shared_ptr> initMode2(new PFInit(grid, 2)); + std::shared_ptr> initMode2(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f, 2)); K::ParticleFilterMixing mode2(Settings::numParticles, initMode2, Settings::Mode2::modeProbability); mode2.setTransition(std::shared_ptr(new PFTransSimple(grid))); mode2.setEvaluation(std::shared_ptr(new PFEval(WiFiModel, beaconModel, grid))); @@ -281,6 +285,7 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector 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 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 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 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); //}