diff --git a/CMakeLists.txt b/CMakeLists.txt index 7c213a4..1e98ce8 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 ) diff --git a/Plotti.h b/Plotti.h index 64129a1..a695c13 100644 --- a/Plotti.h +++ b/Plotti.h @@ -22,7 +22,7 @@ #include #include -#include +#include struct Plotti { @@ -112,7 +112,7 @@ struct Plotti { estPathSmoothed.add(est); } - void debugDistribution1(std::vector> samples){ + void debugDistribution1(std::vector> samples){ float min = +9999; float max = -9999; @@ -133,7 +133,7 @@ struct Plotti { gp << "set cbrange [" << min << ":" << max << "]\n"; } - void debugDistribution2(std::vector> samples){ + void debugDistribution2(std::vector> samples){ float min = +9999; float max = -9999; @@ -299,10 +299,10 @@ struct Plotti { } } - template void addParticles(const std::vector>& particles) { + template void addParticles(const std::vector>& particles) { pParticles.clear(); int i = 0; - for (const K::Particle& p : particles) { + for (const SMC::Particle& 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); diff --git a/Settings.h b/Settings.h index de88d42..4b855b5 100644 --- a/Settings.h +++ b/Settings.h @@ -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 diff --git a/filter/KLB.h b/filter/KLB.h index 555b7f5..4bc8146 100644 --- a/filter/KLB.h +++ b/filter/KLB.h @@ -29,31 +29,30 @@ #include #include -#include +//#include -#include -#include -#include -#include +#include +#include +#include +#include -#include -#include -#include -//#include +#include +#include +#include -#include -#include -#include +#include +#include +#include -#include -#include -#include +#include +#include +#include -#include -#include -#include -#include -#include +//#include +//#include +//#include +//#include +#include #include "Structs.h" @@ -61,7 +60,7 @@ #include "Logic.h" #include "../Settings.h" -static double getKernelDensityProbability(std::vector>& particles, MyState state, std::vector>& samplesWifi){ +static double getKernelDensityProbability(std::vector>& particles, MyState state, std::vector>& samplesWifi){ Distribution::KernelDensity parzen([&](MyState state){ int size = particles.size(); @@ -80,11 +79,11 @@ static double getKernelDensityProbability(std::vector>& par std::vector probsParticleV; //just for plottingstuff - std::vector> samplesParticles; + std::vector> samplesParticles; const int step = 4; int i = 0; - for(K::Particle particle : samplesWifi){ + for(SMC::Particle 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>& par double probiwifi = particle.weight; probsWifiV.push_back(probiwifi); - //samplesParticles.push_back(K::Particle(state, probiParticle)); + //samplesParticles.push_back(SMC::Particle(state, probiParticle)); } //make vectors @@ -111,7 +110,7 @@ static double getKernelDensityProbability(std::vector>& par //estimate the mean -// K::ParticleFilterEstimationOrderedWeightedAverage estimateWifi(0.95); +// SMC::ParticleFilterEstimationOrderedWeightedAverage estimateWifi(0.95); // const MyState estWifi = estimateWifi.estimate(samplesWifi); // plot.addEstimationNodeSmoothed(estWifi.position.inMeter()); @@ -119,7 +118,7 @@ static double getKernelDensityProbability(std::vector>& par } -static double kldFromMultivariatNormal(std::vector>& particles, MyState state, std::vector>& particleWifi){ +static double kldFromMultivariatNormal(std::vector>& particles, MyState state, std::vector>& 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>& partic 0, 0, 0.01; //estimate the mean - K::ParticleFilterEstimationOrderedWeightedAverage estimateWifi(0.95); + SMC::ParticleFilterEstimationOrderedWeightedAverage estimateWifi(0.95); const MyState estWifi = estimateWifi.estimate(particleWifi); Eigen::VectorXd meanWifi(3); diff --git a/filter/Logic.h b/filter/Logic.h index 4c14cec..bbf985c 100644 --- a/filter/Logic.h +++ b/filter/Logic.h @@ -26,26 +26,28 @@ #include -#include -#include -#include -#include +#include +#include +#include +#include -#include +#include +#include +#include #include "Structs.h" #include #include "../Settings.h" /** particle-filter init randomly distributed within the building*/ -struct PFInit : public K::ParticleFilterInitializer { +struct PFInit : public SMC::ParticleFilterInitializer { Grid& grid; PFInit(Grid& grid) : grid(grid) {;} - virtual void initialize(std::vector>& particles) override { - for (K::Particle& p : particles) { + virtual void initialize(std::vector>& particles) override { + for (SMC::Particle& p : particles) { int idx = rand() % grid.getNumNodes(); p.state.position = grid[idx]; // random position @@ -60,7 +62,7 @@ struct PFInit : public K::ParticleFilterInitializer { }; /** particle-filter init with fixed position*/ -struct PFInitFixed : public K::ParticleFilterInitializer { +struct PFInitFixed : public SMC::ParticleFilterInitializer { Grid& grid; GridPoint startPos; @@ -69,11 +71,11 @@ struct PFInitFixed : public K::ParticleFilterInitializer { PFInitFixed(Grid& grid, GridPoint startPos, float headingDeg) : grid(grid), startPos(startPos), headingDeg(headingDeg) {;} - virtual void initialize(std::vector>& particles) override { + virtual void initialize(std::vector>& particles) override { Distribution::Normal norm(0.0f, 1.5f); - for (K::Particle& p : particles) { + for (SMC::Particle& p : particles) { GridPoint pos = startPos + GridPoint(norm.draw(),norm.draw(),0.0f); @@ -89,7 +91,7 @@ struct PFInitFixed : public K::ParticleFilterInitializer { }; /** very simple transition model, just scatter normal distributed */ -struct PFTransSimple : public K::ParticleFilterTransition{ +struct PFTransSimple : public SMC::ParticleFilterTransition{ Grid& grid; @@ -106,13 +108,13 @@ struct PFTransSimple : public K::ParticleFilterTransition{ /** ctor */ PFTransSimple(Grid& grid) : grid(grid) {} - virtual void transition(std::vector>& particles, const MyControl* control) override { + virtual void transition(std::vector>& 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& p = particles[i]; + SMC::Particle& 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{ }; /** particle-filter transition */ -struct PFTrans : public K::ParticleFilterTransition { +struct PFTrans : public SMC::ParticleFilterTransition { Grid& grid; @@ -169,7 +171,6 @@ struct PFTrans : public K::ParticleFilterTransition { std::minstd_rand gen; - PFTrans(Grid& 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 { //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>& particles, const MyControl* control) override { + virtual void transition(std::vector>& particles, const MyControl* control) override { std::normal_distribution noise(0, Settings::IMU::stepSigma); - for (K::Particle& p : particles) { + for (SMC::Particle& 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 { * particle-filter transition * Adapting the Sample Size in Particle Filters Through KLD-Sampling - D. Fox */ -struct PFTransKLDSampling : public K::ParticleFilterTransition { +struct PFTransKLDSampling : public SMC::ParticleFilterTransition { Grid& grid; @@ -248,8 +249,8 @@ struct PFTransKLDSampling : public K::ParticleFilterTransition& 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>& particles, const MyControl* control) override { + virtual void transition(std::vector>& particles, const MyControl* control) override { std::normal_distribution noise(0, Settings::IMU::stepSigma); Distribution::Uniform getParticle(0, particles.size()-1); @@ -281,13 +282,13 @@ struct PFTransKLDSampling : public K::ParticleFilterTransition> particlesNew; + std::vector> particlesNew; do{ //draw equally from the particle set int particleIdx = getParticle.draw(); - K::Particle& p = particles[particleIdx]; + SMC::Particle& p = particles[particleIdx]; //sample new particles based on the transition step // save old position @@ -310,7 +311,7 @@ struct PFTransKLDSampling : public K::ParticleFilterTransition::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{ +struct BFTrans : public SMC::BackwardFilterTransition{ public: @@ -362,28 +362,21 @@ public: } } + std::vector> transition(std::vector> const& toBeSmoothedParticles_qt, std::vector const& controls_1T) override{ + Assert::doThrow( "Wrong transition function. Use the other one!"); + + std::vector> 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> transition(std::vector>const& particles_qt, - std::vector>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> transition(std::vector>const& particles_qt, + std::vector>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 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::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::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::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{ - - -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> transition(std::vector>const& particles_new, - std::vector>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> 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 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 { +struct PFEval : public SMC::ParticleFilterEvaluation { WiFiModel& wifiModel; WiFiObserverFree wiFiProbability; // free-calculation @@ -579,7 +480,7 @@ struct PFEval : public K::ParticleFilterEvaluation { return Distribution::Normal::getProbability(static_cast(hPa), 0.10, static_cast(observation.relativePressure)); } - double getStairProb(const K::Particle& p, const Activity act) { + double getStairProb(const SMC::Particle& p, const Activity act) { const float kappa = 0.65; @@ -603,7 +504,7 @@ struct PFEval : public K::ParticleFilterEvaluation { } - virtual double evaluation(std::vector>& particles, const MyObs& observation) override { + virtual double evaluation(std::vector>& 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 { #pragma omp parallel for num_threads(3) for (int i = 0; i < particles.size(); ++i) { - K::Particle& p = particles[i]; + SMC::Particle& 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 { //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)); diff --git a/main.cpp b/main.cpp index c5b0f61..0bcd66b 100755 --- a/main.cpp +++ b/main.cpp @@ -11,7 +11,10 @@ #include #include +#include +#include +#include //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 run(DataSetup setup, int numFile, std::string folder, std::vector gtPath) { +Stats::Statistics run(DataSetup setup, int numFile, std::string folder, std::vector gtPath) { std::vector kld_data; @@ -111,7 +114,7 @@ K::Statistics run(DataSetup setup, int numFile, std::string folder, std:: // build the grid std::ifstream inp(setup.grid, std::ifstream::binary); - Grid grid(20); + Grid 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 run(DataSetup setup, int numFile, std::string folder, std:: MyObs obs; //History of all estimated particles. Used for smoothing - std::vector>> pfHistory; - std::vector tsHistory; + SMC::ForwardFilterHistory pfHistory; //filter init - //K::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInit(grid))); - K::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInitFixed(grid, GridPoint(55.5f * 100.0, 43.7f * 100.0, 740.0f), 180.0f))); + SMC::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInit(grid))); + //SMC::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInitFixed(grid, GridPoint(55.5f * 100.0, 43.7f * 100.0, 740.0f), 180.0f))); pf.setTransition(std::unique_ptr(new PFTrans(grid, &ctrl))); //pf.setTransition(std::unique_ptr(new PFTransKLDSampling(grid, &ctrl))); @@ -166,40 +168,42 @@ K::Statistics run(DataSetup setup, int numFile, std::string folder, std:: //resampling if(Settings::useKLB){ - pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingDivergence())); + pf.setResampling(std::unique_ptr>(new SMC::ParticleFilterResamplingDivergence())); } else { - pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingSimple())); - //pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingPercent(0.4))); + pf.setResampling(std::unique_ptr>(new SMC::ParticleFilterResamplingSimple())); + //pf.setResampling(std::unique_ptr>(new SMC::ParticleFilterResamplingPercent(0.4))); //pf.setResampling(std::unique_ptr>(new NodeResampling(*grid));); - //pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingKLD())); + //pf.setResampling(std::unique_ptr>(new SMC::ParticleFilterResamplingKLD())); } pf.setNEffThreshold(0.95); //estimation - pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); - //pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationRegionalWeightedAverage())); - //pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationOrderedWeightedAverage(0.5))); - //pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationKernelDensity())); + pf.setEstimation(std::unique_ptr>(new SMC::ParticleFilterEstimationWeightedAverage())); + //pf.setEstimation(std::unique_ptr>(new SMC::ParticleFilterEstimationRegionalWeightedAverage())); + //pf.setEstimation(std::unique_ptr>(new SMC::ParticleFilterEstimationOrderedWeightedAverage(0.5))); + //pf.setEstimation(std::unique_ptr>(new SMC::ParticleFilterEstimationKernelDensity())); /** Smoothing Init */ - K::BackwardSimulation bf(Settings::numBSParticles); + SMC::FastKDESmoothing 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>(new K::CumulativeSampler())); + bf.setSampler( std::unique_ptr>(new SMC::CumulativeSampler())); bool smoothing_resample = false; //bf->setNEffThreshold(1.0); if(smoothing_resample) - bf.setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); - bf.setTransition(std::unique_ptr( new BFTrans) ); + bf.setResampling( std::unique_ptr>(new SMC::ParticleFilterResamplingSimple()) ); + + //bf.setTransition(std::unique_ptr( new BFTrans) ); + bf.setTransition(std::unique_ptr(new PFTrans(grid, &ctrl))); //Smoothing estimation - bf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); - //bf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationRegionalWeightedAverage())); - //bf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationOrderedWeightedAverage(0.50f))); + bf.setEstimation(std::unique_ptr>(new SMC::ParticleFilterEstimationWeightedAverage())); + //bf->setEstimation( std::unique_ptr>(new SMC::ParticleFilterEstimationRegionalWeightedAverage())); + //bf->setEstimation( std::unique_ptr>(new SMC::ParticleFilterEstimationOrderedWeightedAverage(0.50f))); } @@ -216,8 +220,8 @@ K::Statistics run(DataSetup setup, int numFile, std::string folder, std:: RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); - K::Statistics errorStats; - K::Statistics errorStatsSmoothing; + Stats::Statistics errorStats; + Stats::Statistics errorStatsSmoothing; //file writing for error data const long int t = static_cast(time(NULL)); @@ -293,8 +297,8 @@ K::Statistics 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 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(pfHistory.getFirstTimestamp().ms())); + } else { + gtPosSmoothed = gtInterpolator.get(static_cast(pfHistory.getTimestamp(Settings::Smoothing::lag).ms())); } - Point3 estPosSmoothing = estBF.position.inMeter(); - Point3 gtPosSmoothed = gtInterpolator.get(static_cast(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 run(DataSetup setup, int numFile, std::string folder, std:: int main(int argc, char** argv) { - K::Statistics statsAVG; - K::Statistics statsMedian; - K::Statistics statsSTD; - K::Statistics statsQuantil; - K::Statistics tmp; + Stats::Statistics statsAVG; + Stats::Statistics statsMedian; + Stats::Statistics statsSTD; + Stats::Statistics statsQuantil; + Stats::Statistics 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;; }