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

2
code/CMakeLists.txt.user
View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 3.6.0, 2016-03-29T16:54:26. -->
<!-- Written by QtCreator 3.6.0, 2016-04-01T17:34:46. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>

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()) );

2
code/particles/MyEvaluation.h
View File

@@ -79,7 +79,7 @@ public:
weight *= turnEval.getProbability(p.state, observation.turn, true);
//set
p.state.measurement_angle = observation.turn->delta_heading;
p.state.angularHeadingChange = observation.turn->delta_heading;
}
// set and accumulate

6
code/particles/MyState.h
View File

@@ -29,8 +29,10 @@ struct MyState {
// save last hPa measurement for the smoothing process
double measurement_pressure;
// save last angle measurement for the smoothing process
double measurement_angle;
// save last angularHeadingChangefor the smoothing process in Degree
double angularHeadingChange;
double avgAngle;
//int distanceWalkedCM;

26
code/particles/smoothing/MySmoothingTransitionExperimental.h
View File

@@ -29,7 +29,8 @@ public:
* @param fp the underlying floorplan
*/
MySmoothingTransitionExperimental() :
distWalk(smoothing_walk_mu, smoothing_walk_sigma) {
distWalk(smoothing_walk_mu, smoothing_walk_sigma)
{
distWalk.setSeed(4321);
}
@@ -75,23 +76,34 @@ public:
//!!!distance kann hier zu groß werden!!!
const double distance_m = p2->state.pCur.getDistance(p1->state.pCur) / 100.0;
//get distance walked and getProb using the walking model
//double distDijkstra_m = ((GRID_DISTANCE_CM / 100.0) * (8 - 1));
const double distProb = distWalk.getProbability(distance_m);
//getProb using the angle(heading) between src and dst
double angle = 0.0;
if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){
angle = Angle::getDEG_360(p2->state.pCur.x, p2->state.pCur.y, p1->state.pCur.x, p1->state.pCur.y);
}
// double angle = 0.0;
// if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){
// angle = Angle::getDEG_360(p2->state.pCur.x, p2->state.pCur.y, p1->state.pCur.x, p1->state.pCur.y);
// }
const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle);
// const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle);
// is the heading change similiar to the measurement?
double p2AngleDeg = p2->state.walkState.heading.getRAD() * 180/3.14159265359;
double p1AngleDeg = p1->state.walkState.heading.getRAD() * 180/3.14159265359;
double diffDeg = p2AngleDeg - p1AngleDeg;
const double headingProb = K::NormalDistribution::getProbability(p1->state.angularHeadingChange, smoothing_heading_sigma, diffDeg);
//assert(headingProb != 0.0);
//assert(distProb != 0.0);
//check how near we are to the measurement
double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa);
@@ -102,7 +114,7 @@ public:
if(distance_m != distance_m) {throw "detected NaN";}
if(distProb != distProb) {throw "detected NaN";}
if(angle != angle) {throw "detected NaN";}
// if(angle != angle) {throw "detected NaN";}
if(headingProb != headingProb) {throw "detected NaN";}
if(floorProb != floorProb) {throw "detected NaN";}
if(floorProb == 0) {throw "detected NaN";}