diff --git a/code/CMakeLists.txt.user b/code/CMakeLists.txt.user index 8b9a790..43b509b 100644 --- a/code/CMakeLists.txt.user +++ b/code/CMakeLists.txt.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/code/Settings.h b/code/Settings.h index 839c6a3..7be540c 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -17,9 +17,9 @@ namespace MiscSettings { const int timeSteps = 500; - const int numParticles = 2500; + const int numParticles = 50; - const int numBSParticles = 500; + const int numBSParticles = 25; const int lag = 5; diff --git a/code/eval/FixedLagEvalBase.h b/code/eval/FixedLagEvalBase.h index c16f99f..74885c6 100644 --- a/code/eval/FixedLagEvalBase.h +++ b/code/eval/FixedLagEvalBase.h @@ -330,8 +330,8 @@ public: statsDistFiltering.add(errDist); errorsNorm.push_back(err); errorsDistNorm.push_back(errDist); - std::cout << "FilteringTime: " << se.ts << " " << statsFiltering.asString() << std::endl; - std::cout << "FilteringDist: " << se.ts << " " << statsDistFiltering.asString() << std::endl; + //std::cout << "FilteringTime: " << se.ts << " " << statsFiltering.asString() << std::endl; + //std::cout << "FilteringDist: " << se.ts << " " << statsDistFiltering.asString() << std::endl; } if(cnt > skip){ @@ -377,8 +377,8 @@ public: errorsSmooth.push_back(errSmoothed); errorsDistSmooth.push_back(errDistSmoothed); - std::cout << "SmoothingTime: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsSmoothing.asString() << std::endl; - std::cout << "SmoothingDist: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsDistSmoothing.asString() << std::endl; + //std::cout << "SmoothingTime: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsSmoothing.asString() << std::endl; + //std::cout << "SmoothingDist: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsDistSmoothing.asString() << std::endl; //plot vis.clearStates(); @@ -420,10 +420,10 @@ public: //Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05; vis.gp << "set arrow 999 from screen " << p1.x<<","<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::ParticleFilterEstimationOrderedWeightedAverage(0.50f))); //create the backward smoothing filter @@ -80,7 +80,7 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.05; - bool smoothing_resample = false; + bool smoothing_resample = true; //Smoothing using Simple Trans @@ -115,6 +115,7 @@ public: //Smoothing using Simple Trans + //bf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationOrderedWeightedAverage(0.50f))); bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverageWithAngle())); if(smoothing_resample) bf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); @@ -145,7 +146,7 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.05; - bool smoothing_resample = false; + bool smoothing_resample = true; //Smoothing using Dijkstra @@ -174,7 +175,7 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.05; - bool smoothing_resample = false; + bool smoothing_resample = true; //Smoothing using Simple Trans @@ -242,7 +243,7 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.05; - bool smoothing_resample = false; + bool smoothing_resample = true; //Smoothing using Simple Trans @@ -311,7 +312,7 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.05; - bool smoothing_resample = false; + bool smoothing_resample = true; //Smoothing using Simple Trans @@ -379,7 +380,7 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.05; - bool smoothing_resample = false; + bool smoothing_resample = true; //Smoothing using Simple Trans @@ -448,7 +449,7 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.15; - bool smoothing_resample = false; + bool smoothing_resample = true; //Smoothing using Simple Trans @@ -499,7 +500,7 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.15; - bool smoothing_resample = false; + bool smoothing_resample = true; //Smoothing using Simple Trans @@ -551,7 +552,7 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.15; - bool smoothing_resample = false; + bool smoothing_resample = true; //Smoothing using Simple Trans @@ -604,7 +605,7 @@ public: smoothing_heading_sigma = 5.0; smoothing_baro_sigma = 0.15; - bool smoothing_resample = false; + bool smoothing_resample = true; //Smoothing using Simple Trans diff --git a/code/eval/SmoothingEvalBase.h b/code/eval/SmoothingEvalBase.h index da34aa2..82c6975 100644 --- a/code/eval/SmoothingEvalBase.h +++ b/code/eval/SmoothingEvalBase.h @@ -313,8 +313,8 @@ public: statsDistFiltering.add(errDist); errorsNorm.push_back(err); errorsDistNorm.push_back(errDist); - std::cout << "FilteringTime: " << se.ts << " " << statsFiltering.asString() << std::endl; - std::cout << "FilteringDist: " << se.ts << " " << statsDistFiltering.asString() << std::endl; + //std::cout << "FilteringTime: " << se.ts << " " << statsFiltering.asString() << std::endl; + //std::cout << "FilteringDist: " << se.ts << " " << statsDistFiltering.asString() << std::endl; //save the current estimation for later smoothing. pfHistory.push_back(pf->getNonResamplingParticles()); @@ -395,8 +395,8 @@ public: errorsSmooth.push_back(errSmoothed); errorsDistSmooth.push_back(errDistSmoothed); - std::cout << "SmoothingTime: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsSmoothing.asString() << std::endl; - std::cout << "SmoothingDist: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsDistSmoothing.asString() << std::endl; + //std::cout << "SmoothingTime: " << tsHistory[t] << " " << statsSmoothing.asString() << std::endl; + //std::cout << "SmoothingDist: " << tsHistory[t] << " " << statsDistSmoothing.asString() << std::endl; // plot @@ -486,6 +486,9 @@ public: //Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05; vis.gp << "set arrow 999 from screen " << p1.x<<","<