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

3
CMakeLists.txt
View File

@@ -58,13 +58,14 @@ ADD_DEFINITIONS(
-fstack-protector-all
-g3
#-O2
-O2
-march=native
-DWITH_TESTS
-DWITH_ASSERTIONS
#-DWITH_DEBUG_LOG
-DWITH_DEBUG_PLOT
#-D_GLIBCXX_DEBUG
)

10
Plotti.h
View File

@@ -22,7 +22,7 @@
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <Indoor/smc/filtering/ParticleFilter.h>
struct Plotti {
@@ -112,7 +112,7 @@ struct Plotti {
estPathSmoothed.add(est);
}
void debugDistribution1(std::vector<K::Particle<MyState>> samples){
void debugDistribution1(std::vector<SMC::Particle<MyState>> samples){
float min = +9999;
float max = -9999;
@@ -133,7 +133,7 @@ struct Plotti {
gp << "set cbrange [" << min << ":" << max << "]\n";
}
void debugDistribution2(std::vector<K::Particle<MyState>> samples){
void debugDistribution2(std::vector<SMC::Particle<MyState>> samples){
float min = +9999;
float max = -9999;
@@ -299,10 +299,10 @@ struct Plotti {
}
}
template <typename State> void addParticles(const std::vector<K::Particle<State>>& particles) {
template <typename State> void addParticles(const std::vector<SMC::Particle<State>>& particles) {
pParticles.clear();
int i = 0;
for (const K::Particle<State>& p : particles) {
for (const SMC::Particle<State>& p : particles) {
if (++i % 25 != 0) {continue;}
K::GnuplotPoint3 pos(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
pParticles.add(pos / 100.0f);

6
Settings.h
View File

@@ -27,15 +27,19 @@ namespace Settings {
}
namespace Smoothing {
const bool activated = false;
const bool activated = true;
const double stepLength = 0.7;
const double stepSigma = 0.2;
const double headingSigma = 25.0;
const double zChange = 0.0; // mu change in height between two time steps
const double zSigma = 0.1;
const int lag = 5;
}
namespace KDE {
const Point2 bandwidth(100,100);
}
//const GridPoint destination = GridPoint(70*100, 35*100, 0*100); // use destination
const GridPoint destination = GridPoint(0,0,0); // do not use destination

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);

199
filter/Logic.h
View File

@@ -26,26 +26,28 @@
#include <Indoor/sensors/activity/ActivityDetector.h>
#include <KLib/math/filter/particles/ParticleFilterMixing.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingKLD.h>
#include <Indoor/smc/filtering/ParticleFilterMixing.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingPercent.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKLD.h>
#include <KLib/math/filter/smoothing/BackwardFilterTransition.h>
#include <Indoor/smc/smoothing/BackwardFilterTransition.h>
#include <Indoor/smc/smoothing/BackwardSimulation.h>
#include <Indoor/smc/sampling/CumulativeSampler.h>
#include "Structs.h"
#include <omp.h>
#include "../Settings.h"
/** particle-filter init randomly distributed within the building*/
struct PFInit : public K::ParticleFilterInitializer<MyState> {
struct PFInit : public SMC::ParticleFilterInitializer<MyState> {
Grid<MyNode>& grid;
PFInit(Grid<MyNode>& grid) : grid(grid) {;}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
for (K::Particle<MyState>& p : particles) {
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
for (SMC::Particle<MyState>& p : particles) {
int idx = rand() % grid.getNumNodes();
p.state.position = grid[idx]; // random position
@@ -60,7 +62,7 @@ struct PFInit : public K::ParticleFilterInitializer<MyState> {
};
/** particle-filter init with fixed position*/
struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
struct PFInitFixed : public SMC::ParticleFilterInitializer<MyState> {
Grid<MyNode>& grid;
GridPoint startPos;
@@ -69,11 +71,11 @@ struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
PFInitFixed(Grid<MyNode>& grid, GridPoint startPos, float headingDeg) :
grid(grid), startPos(startPos), headingDeg(headingDeg) {;}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
Distribution::Normal<float> norm(0.0f, 1.5f);
for (K::Particle<MyState>& p : particles) {
for (SMC::Particle<MyState>& p : particles) {
GridPoint pos = startPos + GridPoint(norm.draw(),norm.draw(),0.0f);
@@ -89,7 +91,7 @@ struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
};
/** very simple transition model, just scatter normal distributed */
struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
struct PFTransSimple : public SMC::ParticleFilterTransition<MyState, MyControl>{
Grid<MyNode>& grid;
@@ -106,13 +108,13 @@ struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
/** ctor */
PFTransSimple(Grid<MyNode>& grid) : grid(grid) {}
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
virtual void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
//int noNewPositionCounter = 0;
#pragma omp parallel for num_threads(6)
for (int i = 0; i < particles.size(); ++i) {
K::Particle<MyState>& p = particles[i];
SMC::Particle<MyState>& p = particles[i];
// update the baromter
float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
@@ -151,7 +153,7 @@ struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
};
/** particle-filter transition */
struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
struct PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
Grid<MyNode>& grid;
@@ -169,7 +171,6 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
std::minstd_rand gen;
PFTrans(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma), modHeadMises(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) {
walker.addModule(&modHead);
@@ -177,18 +178,18 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
//walker.addModule(&modSpread); // might help in some situations! keep in mind!
//walker.addModule(&modActivity);
//walker.addModule(&modHeadUgly);
walker.addModule(&modImportance);
//walker.addModule(&modImportance);
//walker.addModule(&modFavorZ);
//walker.addModule(&modButterAct);
//walker.addModule(&modWifi);
//walker.addModule(&modPreventVisited);
}
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
virtual void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
for (K::Particle<MyState>& p : particles) {
for (SMC::Particle<MyState>& p : particles) {
//this is just for the smoothing transition... quick and dirty
p.state.headingChangeMeasured_rad = control->turnSinceLastTransition_rad;
@@ -215,7 +216,7 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
* particle-filter transition
* Adapting the Sample Size in Particle Filters Through KLD-Sampling - D. Fox
*/
struct PFTransKLDSampling : public K::ParticleFilterTransition<MyState, MyControl> {
struct PFTransKLDSampling : public SMC::ParticleFilterTransition<MyState, MyControl> {
Grid<MyNode>& grid;
@@ -248,8 +249,8 @@ struct PFTransKLDSampling : public K::ParticleFilterTransition<MyState, MyContro
PFTransKLDSampling(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma), modHeadMises(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) {
walker.addModule(&modHead);
//walker.addModule(&modHeadMises);
//walker.addModule(&modHead);
walker.addModule(&modHeadMises);
//walker.addModule(&modSpread); // might help in some situations! keep in mind!
//walker.addModule(&modActivity);
//walker.addModule(&modHeadUgly);
@@ -267,7 +268,7 @@ struct PFTransKLDSampling : public K::ParticleFilterTransition<MyState, MyContro
bins.setRanges({BinningRange(-1,100), BinningRange(-10,60), BinningRange(-1,15), BinningRange(0, 2 * M_PI)});
}
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
virtual void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
Distribution::Uniform<int> getParticle(0, particles.size()-1);
@@ -281,13 +282,13 @@ struct PFTransKLDSampling : public K::ParticleFilterTransition<MyState, MyContro
bins.clearUsed();
//create new particle set
std::vector<K::Particle<MyState>> particlesNew;
std::vector<SMC::Particle<MyState>> particlesNew;
do{
//draw equally from the particle set
int particleIdx = getParticle.draw();
K::Particle<MyState>& p = particles[particleIdx];
SMC::Particle<MyState>& p = particles[particleIdx];
//sample new particles based on the transition step
// save old position
@@ -310,7 +311,7 @@ struct PFTransKLDSampling : public K::ParticleFilterTransition<MyState, MyContro
bins.markUsed(p.state);
//calculate the new N
double z_delta = K::NormalDistributionCDF::getProbit(1 - delta);
double z_delta = Distribution::NormalCDF<double>::getProbit(1 - delta);
double front = (k - 1) / (2 * epsilon);
double back = 1 - (2 / (9 * (k - 1))) + (std::sqrt(2 / (9 * (k - 1))) * z_delta );
@@ -332,8 +333,7 @@ struct PFTransKLDSampling : public K::ParticleFilterTransition<MyState, MyContro
};
struct BFTrans : public K::BackwardFilterTransition<MyState>{
struct BFTrans : public SMC::BackwardFilterTransition<MyState, MyControl>{
public:
@@ -362,28 +362,21 @@ public:
}
}
std::vector<SMC::Particle<MyState>> transition(std::vector<SMC::Particle<MyState>> const& toBeSmoothedParticles_qt, std::vector<MyControl> const& controls_1T) override{
Assert::doThrow( "Wrong transition function. Use the other one!");
std::vector<SMC::Particle<MyState>> dummyReturn;
return dummyReturn;
}
/**
* smoothing transition starting at T with t, t-1,...0
* @param particles_qt q_t (Forward Filter) p2
* @param particles_qt1 q_t+1 (Smoothed Particles from Step before) p1
*/
std::vector<std::vector<double>> transition(std::vector<K::Particle<MyState>>const& particles_qt,
std::vector<K::Particle<MyState>>const& particles_qt1,
const MyControl* control) override {
// Forward Transition von q_t nach q_t+1* with tracking of particle using an id = p(q_t+1* | q_t)
//TODO: darf ich das einfach? einfach eine neue Dichte Sampeln? Brauch ich da nicht eine "Erlaubnis" (Two-Filter Smoother kann das)
// law of total probabality auch einfach über ziehen ??
// KDE auf q_t+1 Samples = p(q_t+1 | o_1:T)
// Apply Position from Samples from q_t+1* into KDE of p(q_t+1 | o_1:T) to get p(q_t+1* | o_1:T)
// Calculate new weight w(q_(t|T)) = w(q_t) * p(q_t+1* | o_1:T) * p(q_t+1* | q_t) * normalisation
std::vector<std::vector<double>> transition(std::vector<SMC::Particle<MyState>>const& particles_qt,
std::vector<SMC::Particle<MyState>>const& particles_qt1) override {
// calculate alpha(m,n) = p(q_t+1(m) | q_t(n))
@@ -395,33 +388,33 @@ public:
omp_set_dynamic(0); // Explicitly disable dynamic teams
omp_set_num_threads(7);
#pragma omp parallel for shared(predictionProbabilities)
for (int i = 0; i < particles_old.size(); ++i) {
for (int i = 0; i < particles_qt1.size(); ++i) {
std::vector<double> innerVector;
auto p1 = &particles_old[i];
auto p1 = &particles_qt1[i];
for(int j = 0; j < particles_new.size(); ++j){
for(int j = 0; j < particles_qt.size(); ++j){
auto p2 = &particles_new[j];
auto p2 = &particles_qt[j];
const double distance_m = p2->state.position.inMeter().getDistance(p1->state.position.inMeter()) / 100.0;
//TODO Incorporated Activity - see IPIN16 MySmoothingTransitionExperimental
const double distProb = K::NormalDistribution::getProbability(Settings::Smoothing::stepLength, Settings::Smoothing::stepSigma, distance_m);
const double distProb = Distribution::Normal<double>::getProbability(Settings::Smoothing::stepLength, Settings::Smoothing::stepSigma, distance_m);
// TODO: FIX THIS CORRECTLY is the heading change similiar to the measurement?
double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.heading.direction.getRAD(), p1->state.heading.direction.getRAD());
double diffDeg = Angle::radToDeg(diffRad);
double measurementRad = Angle::makeSafe_2PI(p1->state.headingChangeMeasured_rad);
double measurementDeg = Angle::radToDeg(measurementRad);
const double headingProb = K::NormalDistribution::getProbability(measurementDeg, Settings::Smoothing::headingSigma, diffDeg);
const double headingProb = Distribution::Normal<double>::getProbability(measurementDeg, Settings::Smoothing::headingSigma, diffDeg);
// does the angle between two particles positions is similiar to the measurement
//double angleBetweenParticles = Angle::getDEG_360(p2->state.position.x, p2->state.position.y, p1->state.position.x, p1->state.position.y);
//check how near we are to the measurement
double diffZ = (p2->state.position.inMeter().z - p1->state.position.inMeter().z) / 100.0;
const double floorProb = K::NormalDistribution::getProbability(Settings::Smoothing::zChange, Settings::Smoothing::zSigma, diffZ);
const double floorProb = Distribution::Normal<double>::getProbability(Settings::Smoothing::zChange, Settings::Smoothing::zSigma, diffZ);
//combine the probabilities
double prob = distProb;// * floorProb * headingProb;
@@ -437,99 +430,7 @@ public:
}
};
struct BFTransKDESlow : public K::BackwardFilterTransition<MyState>{
public:
/**
* ctor
* @param choice the choice to use for randomly drawing nodes
* @param fp the underlying floorplan
*/
BFTrans()
{
//nothin
}
uint64_t ts = 0;
uint64_t deltaMS = 0;
/** set the current time in millisconds */
void setCurrentTime(const uint64_t ts) {
if (this->ts == 0) {
this->ts = ts;
deltaMS = 0;
} else {
deltaMS = this->ts - ts;
this->ts = ts;
}
}
/**
* smoothing transition starting at T with t, t-1,...0
* @param particles_new p_t (Forward Filter) p2
* @param particles_old p_t+1 (Smoothed Particles from Step before) p1
* q(p1 | p2) is calculated
*/
std::vector<std::vector<double>> transition(std::vector<K::Particle<MyState>>const& particles_new,
std::vector<K::Particle<MyState>>const& particles_old ) override {
// calculate alpha(m,n) = p(q_t+1(m) | q_t(n))
// this means, predict all possible states q_t+1 with all passible states q_t
// e.g. p(q_490(1)|q_489(1));p(q_490(1)|q_489(2)) ... p(q_490(1)|q_489(N)) and
// p(q_490(1)|q_489(1)); p(q_490(2)|q_489(1)) ... p(q_490(M)|q_489(1))
std::vector<std::vector<double>> predictionProbabilities;
omp_set_dynamic(0); // Explicitly disable dynamic teams
omp_set_num_threads(7);
#pragma omp parallel for shared(predictionProbabilities)
for (int i = 0; i < particles_old.size(); ++i) {
std::vector<double> innerVector;
auto p1 = &particles_old[i];
for(int j = 0; j < particles_new.size(); ++j){
auto p2 = &particles_new[j];
const double distance_m = p2->state.position.inMeter().getDistance(p1->state.position.inMeter()) / 100.0;
//TODO Incorporated Activity - see IPIN16 MySmoothingTransitionExperimental
const double distProb = K::NormalDistribution::getProbability(Settings::Smoothing::stepLength, Settings::Smoothing::stepSigma, distance_m);
// TODO: FIX THIS CORRECTLY is the heading change similiar to the measurement?
double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.heading.direction.getRAD(), p1->state.heading.direction.getRAD());
double diffDeg = Angle::radToDeg(diffRad);
double measurementRad = Angle::makeSafe_2PI(p1->state.headingChangeMeasured_rad);
double measurementDeg = Angle::radToDeg(measurementRad);
const double headingProb = K::NormalDistribution::getProbability(measurementDeg, Settings::Smoothing::headingSigma, diffDeg);
// does the angle between two particles positions is similiar to the measurement
//double angleBetweenParticles = Angle::getDEG_360(p2->state.position.x, p2->state.position.y, p1->state.position.x, p1->state.position.y);
//check how near we are to the measurement
double diffZ = (p2->state.position.inMeter().z - p1->state.position.inMeter().z) / 100.0;
const double floorProb = K::NormalDistribution::getProbability(Settings::Smoothing::zChange, Settings::Smoothing::zSigma, diffZ);
//combine the probabilities
double prob = distProb;// * floorProb * headingProb;
innerVector.push_back(prob);
}
#pragma omp critical
predictionProbabilities.push_back(innerVector);
}
return predictionProbabilities;
}
};
struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
struct PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObs> {
WiFiModel& wifiModel;
WiFiObserverFree wiFiProbability; // free-calculation
@@ -579,7 +480,7 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
return Distribution::Normal<double>::getProbability(static_cast<double>(hPa), 0.10, static_cast<double>(observation.relativePressure));
}
double getStairProb(const K::Particle<MyState>& p, const Activity act) {
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
const float kappa = 0.65;
@@ -603,7 +504,7 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
}
virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObs& observation) override {
virtual double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObs& observation) override {
double sum = 0;
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
@@ -613,7 +514,7 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
K::Particle<MyState>& p = particles[i];
SMC::Particle<MyState>& p = particles[i];
Point3 pos_m = p.state.position.inMeter();
Point3 posOld_m = p.state.positionOld.inMeter();
@@ -625,8 +526,8 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
//const double pBeacon = getBEACON(observation, p.state.position);
//small checks
_assertNotNAN(pWifi, "Wifi prob is nan");
_assertNot0(pBaroPressure,"pBaroPressure is null");
Assert::isNotNaN(pWifi, "Wifi prob is nan");
Assert::isNot0(pBaroPressure,"pBaroPressure is null");
const bool volatile init = observation.currentTime.sec() < 25;
//double pWiFiMod = (init) ? (std::pow(pWiFi, 0.1)) : (std::pow(pWiFi, 0.5));

122
main.cpp
View File

@@ -11,7 +11,10 @@
#include <Indoor/sensors/radio/model/WiFiModelFactory.h>
#include <Indoor/sensors/radio/model/WiFiModelFactoryImpl.h>
#include <Indoor/math/stats/Statistics.h>
#include <Indoor/smc/smoothing/ForwardFilterHistory.h>
#include <Indoor/smc/smoothing/FastKDESmoothing.h>
//frank
//const std::string mapDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/maps/";
@@ -20,8 +23,8 @@
//toni
const std::string mapDir = "/home/toni/Documents/programme/localization/IndoorMap/maps/";
//const std::string dataDir = "/home/toni/Documents/programme/localization/DynLag/code/data/";
//const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/shl/";
const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/motionAxisTest/";
const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/shl/";
//const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/motionAxisTest/";
const std::string errorDir = dataDir + "results/";
/** describes one dataset (map, training, parameter-estimation, ...) */
@@ -87,7 +90,7 @@ struct Data {
Floorplan::IndoorMap* MyState::map;
K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPath) {
Stats::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPath) {
std::vector<double> kld_data;
@@ -111,7 +114,7 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
// build the grid
std::ifstream inp(setup.grid, std::ifstream::binary);
Grid<MyNode> grid(20);
Grid<MyNode> grid(Settings::Grid::gridSize_cm);
// grid.dat empty? -> build one and save it
if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
@@ -152,12 +155,11 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
MyObs obs;
//History of all estimated particles. Used for smoothing
std::vector<std::vector<K::Particle<MyState>>> pfHistory;
std::vector<int64_t> tsHistory;
SMC::ForwardFilterHistory<MyState, MyControl, MyObs> pfHistory;
//filter init
//K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)));
K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInitFixed>(new PFInitFixed(grid, GridPoint(55.5f * 100.0, 43.7f * 100.0, 740.0f), 180.0f)));
SMC::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)));
//SMC::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInitFixed>(new PFInitFixed(grid, GridPoint(55.5f * 100.0, 43.7f * 100.0, 740.0f), 180.0f)));
pf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
//pf.setTransition(std::unique_ptr<PFTransKLDSampling>(new PFTransKLDSampling(grid, &ctrl)));
@@ -166,40 +168,42 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
//resampling
if(Settings::useKLB){
pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingDivergence<MyState>>(new K::ParticleFilterResamplingDivergence<MyState>()));
pf.setResampling(std::unique_ptr<SMC::ParticleFilterResamplingDivergence<MyState>>(new SMC::ParticleFilterResamplingDivergence<MyState>()));
} else {
pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
//pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.4)));
pf.setResampling(std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>>(new SMC::ParticleFilterResamplingSimple<MyState>()));
//pf.setResampling(std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>>(new SMC::ParticleFilterResamplingPercent<MyState>(0.4)));
//pf.setResampling(std::unique_ptr<NodeResampling<MyState, MyNode>>(new NodeResampling<MyState, MyNode>(*grid)););
//pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingKLD<MyState>>(new K::ParticleFilterResamplingKLD<MyState>()));
//pf.setResampling(std::unique_ptr<SMC::ParticleFilterResamplingKLD<MyState>>(new SMC::ParticleFilterResamplingKLD<MyState>()));
}
pf.setNEffThreshold(0.95);
//estimation
pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5)));
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationKernelDensity<MyState, 3>>(new K::ParticleFilterEstimationKernelDensity<MyState, 3>()));
pf.setEstimation(std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationWeightedAverage<MyState>()));
//pf.setEstimation(std::unique_ptr<SMC::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
//pf.setEstimation(std::unique_ptr<SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5)));
//pf.setEstimation(std::unique_ptr<SMC::ParticleFilterEstimationKernelDensity<MyState, 3>>(new SMC::ParticleFilterEstimationKernelDensity<MyState, 3>()));
/** Smoothing Init */
K::BackwardSimulation<MyState> bf(Settings::numBSParticles);
SMC::FastKDESmoothing<MyState, MyControl, MyObs> bf(Settings::numParticles, map, Settings::Grid::gridSize_cm, Settings::KDE::bandwidth);
if(Settings::Smoothing::activated){
//create the backward smoothing filter
bf.setSampler( std::unique_ptr<K::CumulativeSampler<MyState>>(new K::CumulativeSampler<MyState>()));
bf.setSampler( std::unique_ptr<SMC::CumulativeSampler<MyState>>(new SMC::CumulativeSampler<MyState>()));
bool smoothing_resample = false;
//bf->setNEffThreshold(1.0);
if(smoothing_resample)
bf.setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
bf.setTransition(std::unique_ptr<BFTrans>( new BFTrans) );
bf.setResampling( std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>>(new SMC::ParticleFilterResamplingSimple<MyState>()) );
//bf.setTransition(std::unique_ptr<BFTrans>( new BFTrans) );
bf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
//Smoothing estimation
bf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//bf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
//bf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.50f)));
bf.setEstimation(std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationWeightedAverage<MyState>()));
//bf->setEstimation( std::unique_ptr<SMC::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
//bf->setEstimation( std::unique_ptr<SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.50f)));
}
@@ -216,8 +220,8 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
RelativePressure relBaro;
relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
K::Statistics<float> errorStats;
K::Statistics<float> errorStatsSmoothing;
Stats::Statistics<float> errorStats;
Stats::Statistics<float> errorStatsSmoothing;
//file writing for error data
const long int t = static_cast<long int>(time(NULL));
@@ -293,8 +297,8 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
plot.setEst(estPos);
plot.setGT(gtPos);
plot.addEstimationNode(estPos);
plot.addParticles(pf.getParticles());
//plot.addEstimationNode(estPos);
//plot.addParticles(pf.getParticles());
/** error calculation stuff */
float err_m = gtPos.getDistance(estPos);
@@ -304,34 +308,36 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
/** smoothing stuff */
if(Settings::Smoothing::activated){
//save the current estimation for later smoothing.
pfHistory.push_back(pf.getNonResamplingParticles());
tsHistory.push_back(ts.ms());
// add everything from the forward step to the history
pfHistory.add(ts, pf.getNonResamplingParticles(), ctrl, obs);
//backward filtering
MyState estBF = est;
if(pfHistory.size() > Settings::Smoothing::lag){
//((BFTrans*)bf.getTransition())->setCurrentTime(tsHistory[(tsHistory.size() - 1) - i]);
MyState estBF = bf.update(pfHistory, Settings::Smoothing::lag);
bf.reset();
//use every n-th (std = 1) particle set of the history within a given lag (std = 5)
for(int i = 0; i <= Settings::Smoothing::lag; ++i){
((BFTrans*)bf.getTransition())->setCurrentTime(tsHistory[(tsHistory.size() - 1) - i]);
estBF = bf.update(pfHistory[(pfHistory.size() - 1) - i]);
}
// get ground truth position at lag time
Point3 estPosSmoothing = estBF.position.inMeter();
Point3 gtPosSmoothed;
if(pfHistory.size() <= Settings::Smoothing::lag){
gtPosSmoothed = gtInterpolator.get(static_cast<uint64_t>(pfHistory.getFirstTimestamp().ms()));
} else {
gtPosSmoothed = gtInterpolator.get(static_cast<uint64_t>(pfHistory.getTimestamp(Settings::Smoothing::lag).ms()));
}
Point3 estPosSmoothing = estBF.position.inMeter();
Point3 gtPosSmoothed = gtInterpolator.get(static_cast<uint64_t>(tsHistory[(tsHistory.size() - 1) - Settings::Smoothing::lag]));
//plot
plot.addEstimationNodeSmoothed(estPosSmoothing);
plot.addParticles(bf.getbackwardParticles().back());
if(Settings::Smoothing::lag >= pfHistory.size()){
// error between GT and smoothing
float errSmoothing_m = gtPosSmoothed.getDistance(estPosSmoothing);
errorStatsSmoothing.add(errSmoothing_m);
errorFileSmoothing << errSmoothing_m << "\n";
}
// error between GT and smoothing
float errSmoothing_m = gtPosSmoothed.getDistance(estPosSmoothing);
errorStatsSmoothing.add(errSmoothing_m);
errorFileSmoothing << errSmoothing_m << "\n";
}
//plot misc
@@ -426,26 +432,26 @@ K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::
int main(int argc, char** argv) {
K::Statistics<float> statsAVG;
K::Statistics<float> statsMedian;
K::Statistics<float> statsSTD;
K::Statistics<float> statsQuantil;
K::Statistics<float> tmp;
Stats::Statistics<float> statsAVG;
Stats::Statistics<float> statsMedian;
Stats::Statistics<float> statsSTD;
Stats::Statistics<float> statsQuantil;
Stats::Statistics<float> tmp;
for(int i = 0; i < 10; ++i){
// tmp = run(data.FloorOneToThree, 0, "Wifi-Dongle-Test", Settings::Path_DongleTest::path4);
// statsMedian.add(tmp.getMedian());
// statsAVG.add(tmp.getAvg());
// statsSTD.add(tmp.getStdDev());
// statsQuantil.add(tmp.getQuantile(0.75));
tmp = run(data.MotionAxisTest, 0, "Motion-Axis-Test", Settings::Path_DongleTest::path1);
tmp = run(data.SecondFloorOnly, 0, "KDE-Smoothing-Test", Settings::Path_DongleTest::path1);
statsMedian.add(tmp.getMedian());
statsAVG.add(tmp.getAvg());
statsSTD.add(tmp.getStdDev());
statsQuantil.add(tmp.getQuantile(0.75));
// tmp = run(data.MotionAxisTest, 0, "Motion-Axis-Test", Settings::Path_DongleTest::path1);
// statsMedian.add(tmp.getMedian());
// statsAVG.add(tmp.getAvg());
// statsSTD.add(tmp.getStdDev());
// statsQuantil.add(tmp.getQuantile(0.75));
std::cout << "Iteration " << i << " completed" << std::endl;;
}