toni/IPIN2016
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

experimental stuff... tryed dynamic step length using the barometric height

Browse Source
This commit is contained in:
toni
2016-04-05 08:14:33 +02:00
parent 705d593e36
commit 65294284d0
6 changed files with 52 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
code/eval/FixedLagEvalBase.h
View File

@@ -193,6 +193,9 @@ public:
//stats file
std::ofstream statsout("/tmp/unsmoothed_" + runName + ".stats");
//heading
double currentHeadingGivenByLukas = 0.0;
// process each single sensor reading
while(sr->hasNext()) {
@@ -251,6 +254,8 @@ public:
obs.turn->delta_motion += _to.delta_motion;
ctrl.headingChange_rad = Angle::degToRad(obs.turn->delta_heading);
currentHeadingGivenByLukas = obs.turn->delta_heading;
}
@@ -267,6 +272,11 @@ public:
const MyState est = pf->update(&ctrl, obs);
const Point3 curEst = est.pCur;
//EXPERIMENTAL: Set all Particle Angles to the estimated angle of the particle set
// if(cnt % 30 == 0){
// pf->setAngle(est.avgAngle * 3.14159265359 / 180);
// }
// error calculation. compare ground-truth to estimation
const int offset = 0;
const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
@@ -334,7 +344,19 @@ public:
if (obs.barometer != nullptr) {
vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n";
}
vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
vis.gp << "set label 111 '" << ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
//double avgAngleRad = estBF.avgAngle * 180/3.14159265359;
//std::cout << "Measurement: "<< std::fmod((-(obs.orientation.values[0] * 180/3.14159265359) + 720 + 30), 360) << std::endl;
//std::cout << "Measurement: "<< std::fmod(((obs.orientation.values[0] * 180/3.14159265359) + 720 + 60), 360) << std::endl;
std::cout << "MeasurementLukas: " << currentHeadingGivenByLukas << std::endl;
std::cout << "EstimationS: " << estBF.avgAngle << std::endl;
std::cout << "EstimationF: " << est.avgAngle << std::endl;
//vis.gp << "set label 113 ' EstAngle:" << avgAngleRad << "' at screen 0.1,0.15\n";
//vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";

8
code/eval/SmoothingEval1.h
View File

@@ -19,7 +19,7 @@
#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/ParticleFilterEstimationWeightedAverageWithAngle.h>
class SmoothingEval1 : public FixedLagEvalBase {
@@ -45,7 +45,7 @@ public:
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.10)) );
// state estimation step
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>>(new K::ParticleFilterEstimationWeightedAverageWithAngle<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.50f)));
@@ -100,14 +100,14 @@ public:
//Smoothing Variables
smoothing_walk_mu = 0.7;
smoothing_walk_sigma = 0.5;
smoothing_heading_sigma = 15.0;
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>>(new K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>()));
if(smoothing_resample)
bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );