added tex

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;