added tex

code/CMakeLists.txt

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

code/filter/KLB.h

@@ -74,6 +74,8 @@ struct ModeProbabilityTransition : public K::MarkovTransitionProbability<MyState
         std::vector<double> probsWifiV;
         std::vector<double> probsParticleV;
 
+        WiFiQualityAnalyzer analyzer;
+
         // mode[0] -> Posterior & mode[1] -> Wifi ---- i know what im doing :)
         for(MyNode node : grid.getNodes()){
             double probParzenPosterior = calcKernelDensity(node, modes[0].getParticles());
@@ -87,6 +89,21 @@ struct ModeProbabilityTransition : public K::MarkovTransitionProbability<MyState
         Eigen::Map<Eigen::VectorXd> probsWifi(&probsWifiV[0], probsWifiV.size());
         Eigen::Map<Eigen::VectorXd> probsParticle(&probsParticleV[0], probsParticleV.size());
 
+        //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>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
 
@@ -94,11 +111,13 @@ struct ModeProbabilityTransition : public K::MarkovTransitionProbability<MyState
         __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 << 1-expKld, expKld, 0, 1;
+        m << expKld, 1.0 - expKld, 1 - qualityWifi, qualityWifi;
 
         return m;
 

code/main.cpp

@@ -201,8 +201,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
     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)));
-    IMMAPF.setMarkovTransitionProbability(std::unique_ptr<ModeProbabilityTransitionNormal>(new ModeProbabilityTransitionNormal(Settings::Mixing::lambda)));
+    IMMAPF.setMarkovTransitionProbability(std::unique_ptr<ModeProbabilityTransition>(new ModeProbabilityTransition(grid, Settings::Mixing::lambda)));
+    //IMMAPF.setMarkovTransitionProbability(std::unique_ptr<ModeProbabilityTransitionNormal>(new ModeProbabilityTransitionNormal(Settings::Mixing::lambda)));
 
 
     Timestamp lastTimestamp = Timestamp::fromMS(0);