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

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;