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

added activity recognition to smoothing transition

Browse Source
This commit is contained in:
toni
2016-04-26 10:12:10 +02:00
parent ed8e37108a
commit f7e817d5e4
13 changed files with 204 additions and 183 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -52,8 +52,9 @@ public:
/**
* smoothing transition starting at T with t, t-1,...0
* @param particles_new p_t (Forward Filter)
* @param particles_old p_t+1 (Smoothed Particles from Step before)
* @param particles_new p_t (Forward Filter) p2
* @param particles_old p_t+1 (Smoothed Particles from Step before) p1
* q(p1 | p2) is calculated
*/
std::vector<std::vector<double>> transition(std::vector<K::Particle<MyState>>const& particles_new,
std::vector<K::Particle<MyState>>const& particles_old ) override {
@@ -76,59 +77,76 @@ public:
auto p2 = &particles_new[j];
//!!!distance kann hier zu groß werden!!!
const double distance_m = p2->state.pCur.getDistance(p1->state.pCur) / 100.0;
double muDistance = 1.0;
double sigmaDistance = 0.5;
//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);
switch (p2->state.currentActivity) {
case Activity::ELEVATOR:
muDistance = 0.0;
sigmaDistance = 0.3;
break;
case Activity::STAIRS_DOWN:
muDistance = 0.5;
sigmaDistance = 0.3;
break;
//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);
// }
case Activity::STAIRS_UP:
muDistance = 0.4;
sigmaDistance = 0.2;
break;
// const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle);
case Activity::STANDING:
muDistance = 0.0;
sigmaDistance = 0.2;
break;
case Activity::WALKING:
muDistance = 1.0;
sigmaDistance = 0.5;
break;
default:
muDistance = 1.0;
sigmaDistance = 0.5;
break;
}
const double distProb = K::NormalDistribution::getProbability(muDistance, sigmaDistance, distance_m);
// 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);
double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.walkState.heading.getRAD(), p1->state.walkState.heading.getRAD());
double diffDeg = Angle::radToDeg(diffRad);
double angularChangeZeroToPi = std::fmod(std::abs(p1->state.angularHeadingChange), 360.0);
const double headingProb = K::NormalDistribution::getProbability(angularChangeZeroToPi, smoothing_heading_sigma, diffDeg);
//check how near we are to the measurement
double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa);
const double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa);
//combine the probabilities
double prob = distProb * headingProb * floorProb;
innerVector.push_back(prob);
if(distance_m != distance_m) {throw "detected NaN";}
if(distProb != distProb) {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";}
if(prob != prob) {throw "detected NaN";}
//assert(prob != 0.0);
//error checks
// if(distance_m != distance_m) {throw "detected NaN";}
// if(distProb != distProb) {throw "detected NaN";}
// if(headingProb != headingProb) {throw "detected NaN";}
// if(floorProb != floorProb) {throw "detected NaN";}
// if(floorProb == 0) {throw "detected zero prob in floorprob";}
// if(prob != prob) {throw "detected NaN";}
//if(prob == 0) {++zeroCounter;}
}
#pragma omp critical
predictionProbabilities.push_back(innerVector);
}
return predictionProbabilities;
}