180 lines
6.8 KiB
C++
180 lines
6.8 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 <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/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/statistics/Statistics.h>
|
|
|
|
#include <Indoor/smc/Particle.h>
|
|
#include <Indoor/smc/filtering/ParticleFilterMixing.h>
|
|
#include <Indoor/smc/filtering/ParticleFilterInitializer.h>
|
|
#include <Indoor/smc/filtering/ParticleFilterHistory.h>
|
|
|
|
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
|
|
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
|
|
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
|
|
|
|
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
|
|
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingPercent.h>
|
|
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingDivergence.h>
|
|
|
|
#include <Indoor/smc/merging/MarkovTransitionProbability.h>
|
|
#include <Indoor/smc/merging/mixing/MixingSamplerDivergency.h>
|
|
#include <Indoor/smc/merging/estimation/JointEstimationPosteriorOnly.h>
|
|
|
|
//#include <Indoor/smc/smoothing/BackwardSimulation.h>
|
|
//#include <Indoor/smc/CondensationBackwardFilter.h>
|
|
//#include <Indoor/smc/smoothing/sampling/ParticleTrajectorieSampler.h>
|
|
//#include <Indoor/smc/smoothing/sampling/CumulativeSampler.h>
|
|
#include <Indoor/smc/smoothing/BackwardFilterTransition.h>
|
|
|
|
#include "Structs.h"
|
|
|
|
#include "../Plotti.h"
|
|
#include "Logic.h"
|
|
#include "../Settings.h"
|
|
|
|
static double getKernelDensityProbability(std::vector<SMC::Particle<MyState>>& particles, MyState state, std::vector<SMC::Particle<MyState>>& samplesWifi){
|
|
|
|
Distribution::KernelDensity<double, MyState> parzen([&](MyState state){
|
|
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(state.position);
|
|
prob += Distribution::Normal<double>::getProbability(0, 100, distance) * particles[i].weight;
|
|
}
|
|
|
|
return prob;
|
|
;});
|
|
|
|
std::vector<double> probsWifiV;
|
|
std::vector<double> probsParticleV;
|
|
|
|
//just for plottingstuff
|
|
std::vector<SMC::Particle<MyState>> samplesParticles;
|
|
|
|
const int step = 4;
|
|
int i = 0;
|
|
for(SMC::Particle<MyState> particle : samplesWifi){
|
|
if(++i % step != 0){continue;}
|
|
MyState state(GridPoint(particle.state.position.x_cm, particle.state.position.y_cm, particle.state.position.z_cm));
|
|
|
|
double probiParticle = parzen.getProbability(state);
|
|
probsParticleV.push_back(probiParticle);
|
|
|
|
double probiwifi = particle.weight;
|
|
probsWifiV.push_back(probiwifi);
|
|
|
|
//samplesParticles.push_back(SMC::Particle<MyState>(state, probiParticle));
|
|
}
|
|
|
|
//make vectors
|
|
Eigen::Map<Eigen::VectorXd> probsWifi(&probsWifiV[0], probsWifiV.size());
|
|
Eigen::Map<Eigen::VectorXd> probsParticle(&probsParticleV[0], probsParticleV.size());
|
|
|
|
//get divergence
|
|
double kld = Divergence::KullbackLeibler<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
|
|
//double kld = Divergence::JensenShannon<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
|
|
|
|
//plotti
|
|
//plot.debugDistribution1(samplesWifi);
|
|
//plot.debugDistribution1(samplesParticles);
|
|
|
|
|
|
//estimate the mean
|
|
// SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
|
|
// const MyState estWifi = estimateWifi.estimate(samplesWifi);
|
|
// plot.addEstimationNodeSmoothed(estWifi.position.inMeter());
|
|
|
|
return kld;
|
|
}
|
|
|
|
|
|
static double kldFromMultivariatNormal(std::vector<SMC::Particle<MyState>>& particles, MyState state, std::vector<SMC::Particle<MyState>>& particleWifi){
|
|
//kld: particle die resampling hatten nehmen und nv daraus schätzen. vergleiche mit wi-fi
|
|
//todo put this in depletionhelper.h
|
|
|
|
Point3 estPos = state.position.inMeter();
|
|
|
|
//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
|
|
std::mt19937_64 rng;
|
|
// initialize the random number generator with time-dependent seed
|
|
uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
|
|
std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed>>32)};
|
|
rng.seed(ss);
|
|
// initialize a uniform distribution between -0.0001 and 0.0001
|
|
std::uniform_real_distribution<double> unif(-0.0001, 0.0001);
|
|
|
|
//create a gauss dist for the current particle approx.
|
|
Eigen::MatrixXd m(particles.size(), 3);
|
|
for(int i = 0; i < particles.size(); ++i){
|
|
m(i,0) = (particles[i].state.position.x_cm / 100.0) + unif(rng);
|
|
m(i,1) = (particles[i].state.position.y_cm / 100.0) + unif(rng);
|
|
m(i,2) = (particles[i].state.position.z_cm / 100.0) + unif(rng);
|
|
}
|
|
|
|
Eigen::VectorXd mean(3);
|
|
mean << estPos.x, estPos.y, estPos.z;
|
|
|
|
Distribution::NormalDistributionN normParticle = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(m, mean);
|
|
|
|
//create a gauss dist for wifi
|
|
Eigen::MatrixXd covWifi(3,3);
|
|
covWifi << Settings::WiFiModel::sigma, 0, 0,
|
|
0, Settings::WiFiModel::sigma, 0,
|
|
0, 0, 0.01;
|
|
|
|
//estimate the mean
|
|
SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
|
|
const MyState estWifi = estimateWifi.estimate(particleWifi);
|
|
|
|
Eigen::VectorXd meanWifi(3);
|
|
meanWifi << estWifi.position.x_cm / 100.0, estWifi.position.y_cm / 100.0, estWifi.position.z_cm / 100.0;
|
|
Distribution::NormalDistributionN normWifi(meanWifi, covWifi);
|
|
|
|
//get the kld distance
|
|
double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);
|
|
|
|
//plot.debugDistribution1(particleWifi);
|
|
|
|
//plot.drawNormalN1(normParticle);
|
|
//plot.drawNormalN2(normWifi);
|
|
|
|
//plot.addEstimationNodeSmoothed(estWifi.position.inMeter());
|
|
|
|
return kld;
|
|
}
|
|
|
|
|
|
|
|
#endif // KLB_H
|