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

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