diff --git a/code/Settings.h b/code/Settings.h index 7be540c..c9cc6f0 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -15,9 +15,9 @@ namespace MiscSettings { const int gridSize_cm = 20; - const int timeSteps = 500; + const int timeSteps = 500; - const int numParticles = 50; + const int numParticles = 2500; const int numBSParticles = 25; diff --git a/code/eval/SmoothingEval1.h b/code/eval/SmoothingEval1.h index 78112b9..ed8ad4c 100644 --- a/code/eval/SmoothingEval1.h +++ b/code/eval/SmoothingEval1.h @@ -45,15 +45,23 @@ public: //pf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingPercent(0.10)) ); // state estimation step - pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverageWithAngle())); - //pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationRegionalWeightedAverage())); - //pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationOrderedWeightedAverage(0.50f))); + //pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverageWithAngle())); + //pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationRegionalWeightedAverage())); + pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationOrderedWeightedAverage(0.50f))); //create the backward smoothing filter - bf = new K::BackwardSimulation(MiscSettings::numBSParticles); - //bf = new K::CondensationBackwardFilter; - bf->setSampler( std::unique_ptr>(new K::CumulativeSampler())); + //bf = new K::BackwardSimulation(MiscSettings::numBSParticles); + bf = new K::CondensationBackwardFilter; + //bf->setSampler( std::unique_ptr>(new K::CumulativeSampler())); + + bool smoothing_resample = false; + + bf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationOrderedWeightedAverage(0.50f))); + //bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); + if(smoothing_resample) + bf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); + bf->setTransition(std::unique_ptr( new MySmoothingTransitionExperimental) ); } @@ -80,7 +88,7 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.05; - bool smoothing_resample = true; + bool smoothing_resample = false; //Smoothing using Simple Trans @@ -115,6 +123,7 @@ public: //Smoothing using Simple Trans + //bf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationRegionalWeightedAverage())); //bf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationOrderedWeightedAverage(0.50f))); bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverageWithAngle())); if(smoothing_resample) @@ -146,7 +155,7 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.05; - bool smoothing_resample = true; + bool smoothing_resample = false; //Smoothing using Dijkstra @@ -175,14 +184,6 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.05; - bool smoothing_resample = true; - - - //Smoothing using Simple Trans - bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); - if(smoothing_resample) - bf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); - bf->setTransition(std::unique_ptr( new MySmoothingTransitionExperimental) ); } void bergwerk_path1_nexus_simple() { @@ -243,14 +244,6 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.05; - bool smoothing_resample = true; - - - //Smoothing using Simple Trans - bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); - if(smoothing_resample) - bf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); - bf->setTransition(std::unique_ptr( new MySmoothingTransitionExperimental) ); } void bergwerk_path2_nexus_simple() { @@ -312,14 +305,6 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.05; - bool smoothing_resample = true; - - - //Smoothing using Simple Trans - bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); - if(smoothing_resample) - bf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); - bf->setTransition(std::unique_ptr( new MySmoothingTransitionExperimental) ); } void bergwerk_path3_nexus_simple() { @@ -380,14 +365,6 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.05; - bool smoothing_resample = true; - - - //Smoothing using Simple Trans - bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); - if(smoothing_resample) - bf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); - bf->setTransition(std::unique_ptr( new MySmoothingTransitionExperimental) ); } void bergwerk_path4_nexus_simple() { @@ -449,14 +426,6 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.15; - bool smoothing_resample = true; - - - //Smoothing using Simple Trans - bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); - if(smoothing_resample) - bf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); - bf->setTransition(std::unique_ptr( new MySmoothingTransitionExperimental) ); } void bergwerk_path1_galaxy_simple() { @@ -500,14 +469,6 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.15; - bool smoothing_resample = true; - - - //Smoothing using Simple Trans - bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); - if(smoothing_resample) - bf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); - bf->setTransition(std::unique_ptr( new MySmoothingTransitionExperimental) ); } void bergwerk_path2_galaxy_simple() { @@ -552,14 +513,6 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.15; - bool smoothing_resample = true; - - - //Smoothing using Simple Trans - bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); - if(smoothing_resample) - bf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); - bf->setTransition(std::unique_ptr( new MySmoothingTransitionExperimental) ); } void bergwerk_path3_galaxy_simple() { @@ -605,14 +558,6 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.15; - bool smoothing_resample = true; - - - //Smoothing using Simple Trans - bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); - if(smoothing_resample) - bf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); - bf->setTransition(std::unique_ptr( new MySmoothingTransitionExperimental) ); } void bergwerk_path4_galaxy_simple() { diff --git a/code/eval/SmoothingEvalBase.h b/code/eval/SmoothingEvalBase.h index 82c6975..a6c5d43 100644 --- a/code/eval/SmoothingEvalBase.h +++ b/code/eval/SmoothingEvalBase.h @@ -344,10 +344,10 @@ public: //Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05; vis.gp << "set arrow 999 from screen " << p1.x<<","<