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

removed Heading::rnd()

Browse Source 
changed particle init
fixed uninitialized values
new ground-truth helper methods
added new stats to the eval (distance from ground-truth)
This commit is contained in:
kazu
2016-04-26 11:55:13 +02:00
parent f7e817d5e4
commit 9eb774f7b9
6 changed files with 50 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
code/eval/FixedLagEvalBase.h
View File

@@ -196,6 +196,8 @@ public:
K::Statistics<double> statsFiltering;
K::Statistics<double> statsSmoothing;
K::Statistics<float> statsDistFiltering;
K::Statistics<float> statsDistSmoothing;
int cnt = 0;
// error per update-step
@@ -245,12 +247,12 @@ public:
break;
}
case s_accel: {
float acc[3];
SensorReaderAccel sre; sre.read(se, acc);
actDet.addAccel(acc);
break;
}
case s_accel: {
float acc[3];
SensorReaderAccel sre; sre.read(se, acc);
actDet.addAccel(acc);
break;
}
// case s_linearAcceleration:{
// baroSensorReader.readVerticalAcceleration(se);
@@ -297,7 +299,6 @@ public:
// timed updates
((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS);
// update the particle filter (transition + eval), estimate a new current position and add it to the estimated path
const MyState est = pf->update(&ctrl, obs);
const Point3 curEst = est.pCur;
@@ -315,6 +316,7 @@ public:
pathEst.push_back(curEst);
const float err = diff.length();
const float errDist = gtw.getMinDist(curEst); // minimum distance between estimation and ground-truth
@@ -323,8 +325,10 @@ public:
if (cnt > 35) {
statsFiltering.add(err);
statsDistFiltering.add(errDist);
errorsNorm.push_back(err);
std::cout << "Filtering: " << se.ts << " " << statsFiltering.asString() << std::endl;
std::cout << "FilteringTime: " << se.ts << " " << statsFiltering.asString() << std::endl;
std::cout << "FilteringDist: " << se.ts << " " << statsDistFiltering.asString() << std::endl;
}
if(cnt > skip){
@@ -363,10 +367,14 @@ public:
const float errSmoothed = diffSmoothed.length();
const float errDistSmoothed = gtw.getMinDist(curSmoothedEst); // minimum distance between smoothed-estimation and ground-truth
statsSmoothing.add(errSmoothed);
statsDistSmoothing.add(errDistSmoothed);
errorsSmooth.push_back(errSmoothed);
std::cout << "Smoothing: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsSmoothing.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();