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