FHWS/museumLoc
Archived
3
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

commit before new model is implemented

Browse Source
This commit is contained in:
toni
2018-01-17 13:19:16 +01:00
parent bea81eab62
commit f4c598299f
6 changed files with 152 additions and 241 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
filter/KLB.h
View File

@@ -29,31 +29,30 @@
#include <Indoor/math/divergence/JensenShannon.h>
#include <Indoor/data/Timestamp.h>
#include <KLib/math/statistics/Statistics.h>
//#include <KLib/math/statistics/Statistics.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/ParticleFilterHistory.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 <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 <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.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 <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingPercent.h>
#include <Indoor/smc/filtering/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 <Indoor/smc/merging/MarkovTransitionProbability.h>
#include <Indoor/smc/merging/mixing/MixingSamplerDivergency.h>
#include <Indoor/smc/merging/estimation/JointEstimationPosteriorOnly.h>
#include <KLib/math/filter/smoothing/BackwardSimulation.h>
#include <KLib/math/filter/smoothing/CondensationBackwardFilter.h>
#include <KLib/math/filter/smoothing/sampling/ParticleTrajectorieSampler.h>
#include <KLib/math/filter/smoothing/sampling/CumulativeSampler.h>
#include <KLib/math/filter/smoothing/BackwardFilterTransition.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"
@@ -61,7 +60,7 @@
#include "Logic.h"
#include "../Settings.h"
static double getKernelDensityProbability(std::vector<K::Particle<MyState>>& particles, MyState state, std::vector<K::Particle<MyState>>& samplesWifi){
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();
@@ -80,11 +79,11 @@ static double getKernelDensityProbability(std::vector<K::Particle<MyState>>& par
std::vector<double> probsParticleV;
//just for plottingstuff
std::vector<K::Particle<MyState>> samplesParticles;
std::vector<SMC::Particle<MyState>> samplesParticles;
const int step = 4;
int i = 0;
for(K::Particle<MyState> particle : samplesWifi){
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));
@@ -94,7 +93,7 @@ static double getKernelDensityProbability(std::vector<K::Particle<MyState>>& par
double probiwifi = particle.weight;
probsWifiV.push_back(probiwifi);
//samplesParticles.push_back(K::Particle<MyState>(state, probiParticle));
//samplesParticles.push_back(SMC::Particle<MyState>(state, probiParticle));
}
//make vectors
@@ -111,7 +110,7 @@ static double getKernelDensityProbability(std::vector<K::Particle<MyState>>& par
//estimate the mean
// K::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
// SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
// const MyState estWifi = estimateWifi.estimate(samplesWifi);
// plot.addEstimationNodeSmoothed(estWifi.position.inMeter());
@@ -119,7 +118,7 @@ static double getKernelDensityProbability(std::vector<K::Particle<MyState>>& par
}
static double kldFromMultivariatNormal(std::vector<K::Particle<MyState>>& particles, MyState state, std::vector<K::Particle<MyState>>& particleWifi){
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
@@ -155,7 +154,7 @@ static double kldFromMultivariatNormal(std::vector<K::Particle<MyState>>& partic
0, 0, 0.01;
//estimate the mean
K::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
const MyState estWifi = estimateWifi.estimate(particleWifi);
Eigen::VectorXd meanWifi(3);