223 lines
8.2 KiB
C++
223 lines
8.2 KiB
C++
#ifndef KLB_H
|
|
#define KLB_H
|
|
|
|
#include <chrono>
|
|
|
|
#include <Indoor/math/divergence/KullbackLeibler.h>
|
|
#include <Indoor/grid/factory/v2/GridFactory.h>
|
|
|
|
#include <Indoor/floorplan/v2/Floorplan.h>
|
|
#include <Indoor/floorplan/v2/FloorplanReader.h>
|
|
|
|
#include <Indoor/grid/factory/v2/GridFactory.h>
|
|
#include <Indoor/grid/factory/v2/Importance.h>
|
|
|
|
#include <Indoor/geo/Heading.h>
|
|
#include <Indoor/geo/Point2.h>
|
|
#include <Indoor/sensors/offline/FileReader.h>
|
|
|
|
#include <KLib/math/statistics/Statistics.h>
|
|
|
|
#include <Indoor/sensors/imu/TurnDetection.h>
|
|
#include <Indoor/sensors/imu/StepDetection.h>
|
|
#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>
|
|
#include <Indoor/math/FixedFrequencyInterpolator.h>
|
|
#include <Indoor/math/divergence/KullbackLeibler.h>
|
|
#include <Indoor/math/divergence/JensenShannon.h>
|
|
#include <Indoor/data/Timestamp.h>
|
|
|
|
#include <KLib/math/filter/particles/Particle.h>
|
|
#include <KLib/math/filter/particles/ParticleFilterMixing.h>
|
|
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
|
|
|
|
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
|
|
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
|
|
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
|
|
//#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationKernelDensity.h>
|
|
|
|
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
|
|
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
|
|
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingDivergence.h>
|
|
|
|
#include <KLib/math/filter/merging/MarkovTransitionProbability.h>
|
|
#include <KLib/math/filter/merging/mixing/MixingSamplerDivergency.h>
|
|
#include <KLib/math/filter/merging/estimation/JointEstimationPosteriorOnly.h>
|
|
|
|
#include "Structs.h"
|
|
|
|
#include "../Plotti.h"
|
|
#include "Logic.h"
|
|
#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 :( :( :(
|
|
|
|
|
|
struct ModeProbabilityTransition : public K::MarkovTransitionProbability<MyState, MyControl, MyObs>{
|
|
|
|
Grid<MyNode>& grid;
|
|
const double lambda;
|
|
|
|
ModeProbabilityTransition(Grid<MyNode>& grid, double lambda) : grid(grid), lambda(lambda) {;}
|
|
|
|
virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes, const MyObs& obs) override {
|
|
|
|
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());
|
|
probsParticleV.push_back(probParzenPosterior);
|
|
|
|
double probParzenWifi = calcKernelDensity(node, modes[1].getParticles());
|
|
probsWifiV.push_back(probParzenWifi);
|
|
}
|
|
|
|
// make vectors
|
|
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);
|
|
|
|
// debugging global variable
|
|
__KLD = kld;
|
|
|
|
//exp. distribution
|
|
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.0 - expKld, 1 - qualityWifi, qualityWifi;
|
|
|
|
return m;
|
|
|
|
}
|
|
|
|
double calcKernelDensity(const MyNode node, const std::vector<K::Particle<MyState>> particles){
|
|
|
|
int size = particles.size();
|
|
double prob = 0;
|
|
|
|
#pragma omp parallel for reduction(+:prob) num_threads(6)
|
|
for(int i = 0; i < size; ++i){
|
|
double distance = particles[i].state.position.getDistanceInCM(node);
|
|
prob += Distribution::Normal<double>::getProbability(0, 100, distance) * particles[i].weight;
|
|
}
|
|
|
|
return prob;
|
|
}
|
|
};
|
|
|
|
|
|
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
|
|
Distribution::Uniform<float> uniRand = Distribution::Uniform<float>(-0.1, 0.1);
|
|
|
|
/** ctor */
|
|
ModeProbabilityTransitionNormal(double lambda) : lambda(lambda) {;}
|
|
|
|
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!");
|
|
|
|
// create eigen matrix for posterior and wifi
|
|
Eigen::MatrixXd mParticle(modes[0].getParticles().size(), 3);
|
|
Eigen::MatrixXd mWifi(modes[1].getParticles().size(), 3);
|
|
|
|
#pragma omp parallel for num_threads(6)
|
|
for(int i = 0; i < modes[0].getParticles().size(); ++i){
|
|
mParticle(i,0) = (modes[0].getParticles()[i].state.position.x_cm / 100.0) + uniRand.draw();
|
|
mParticle(i,1) = (modes[0].getParticles()[i].state.position.y_cm / 100.0) + uniRand.draw();
|
|
mParticle(i,2) = (modes[0].getParticles()[i].state.position.z_cm / 100.0) + uniRand.draw();
|
|
|
|
mWifi(i,0) = (modes[1].getParticles()[i].state.position.x_cm / 100.0) + uniRand.draw();
|
|
mWifi(i,1) = (modes[1].getParticles()[i].state.position.y_cm / 100.0) + uniRand.draw();
|
|
mWifi(i,2) = (modes[1].getParticles()[i].state.position.z_cm / 100.0) + uniRand.draw();
|
|
}
|
|
|
|
// create normal distributions
|
|
Eigen::VectorXd meanParticle(3);
|
|
Point3 estParticle = modes[0].getEstimation().position.inMeter();
|
|
meanParticle << estParticle.x, estParticle.y, estParticle.z;
|
|
Distribution::NormalDistributionN normParticle = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(mParticle, meanParticle);
|
|
|
|
Eigen::VectorXd meanWifi(3);
|
|
Point3 estWifi = modes[1].getEstimation().position.inMeter();
|
|
meanWifi << estWifi.x, estWifi.y, estWifi.z;
|
|
Distribution::NormalDistributionN normWifi = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(mWifi, meanWifi);
|
|
|
|
//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 * qualityWifi));
|
|
|
|
Assert::isTrue(expKld < 1.0, "exp. distribution greater 1!");
|
|
|
|
|
|
|
|
//create the matrix
|
|
Eigen::MatrixXd m(2,2);
|
|
m << expKld, 1.0 - expKld, 1 - qualityWifi, qualityWifi;
|
|
|
|
return m;
|
|
|
|
}
|
|
};
|
|
|
|
|
|
#endif // KLB_H
|