FHWS/Fusion2016
Archived
3
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

changed visualisation

Browse Source 
added new eval code for new walkers
improved barometer (moving avg and median)
floorplan-staircase-fixes
disabled step-turn (now part of transition)
added abs-orientation-reader (for testing)
added beacons
This commit is contained in:
FrankE
2016-02-05 20:21:46 +01:00
parent 9e9b6882cd
commit 716b004f3c
15 changed files with 188 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
code/lukas/StepEvaluation.h
View File

@@ -21,48 +21,27 @@ public:
return 1;
const float mdlWalked_m = state.walkState.distanceWalked_m;
// see: particle-filter-control-data
// const float mdlWalked_m = state.walkState.distanceWalked_m;
((MyState&)state).walkState.distanceWalked_m = 0;
// ((MyState&)state).walkState.distanceWalked_m = 0;
const float stepSize_m = 0.71;
const float sensSigma_m = 0.05 + (0.05 * obs->steps);
const float sensWalked_m = obs->steps * stepSize_m;
// const float stepSize_m = 0.71;
// const float sensSigma_m = 0.05 + (0.05 * obs->steps);
// const float sensWalked_m = obs->steps * stepSize_m;
if (obs->steps > 1) {
int i = 0;
int j = i+1; ++j;
}
const double prob = K::NormalDistribution::getProbability(sensWalked_m, sensSigma_m, mdlWalked_m);
if (prob != prob) {
throw 1;
}
return prob;
// float a = 1.0;
// float mu_distance = 0;
// float sigma_distance = 0;
// if(obs->step) {
// a = 1.0;
// mu_distance = mu_walk;
// sigma_distance = sigma_walk;
// if (obs->steps > 1) {
// int i = 0;
// int j = i+1; ++j;
// }
// else {
// a = 0.0;
// mu_distance = mu_stop;
// sigma_distance = sigma_stop;
// const double prob = K::NormalDistribution::getProbability(sensWalked_m, sensSigma_m, mdlWalked_m);
// if (prob != prob) {
// throw 1;
// }
// //Mixed Gaussian model: 1st Gaussian = step, 2nd Gaussian = no step
// const double p = a * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance) +
// (1.0-a) * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance);
// return p;
// return prob;
}

51
code/lukas/TurnEvaluation.h
View File

@@ -19,49 +19,22 @@ public:
return 1;
// get the current heading-change
const float delta = Angle::radToDeg(state.walkState.headingChange_rad);
// see: particle-filter-control-data
// // get the current heading-change
// const float delta = Angle::radToDeg(state.walkState.headingChange_rad);
// and reset it as it was evaluated
((MyState&) state).walkState.headingChange_rad = 0;
// // and reset it as it was evaluated
// ((MyState&) state).walkState.headingChange_rad = 0;
// proability?
const float sigma = 15.0;
const double prob = K::NormalDistribution::getProbability(obs->delta_heading, sigma, delta);
// // proability?
// const float sigma = 15.0;
// const double prob = K::NormalDistribution::getProbability(obs->delta_heading, sigma, delta);
if (prob != prob) {
throw 1;
}
// if (prob != prob) {
// throw 1;
// }
return prob;
// //Particle's heading change
// double delta_heading_particle = state.heading - state.heading_old;
// //Correct offset of the heading change
// if (delta_heading_particle < -180) {
// delta_heading_particle += 360;
// }
// else if (delta_heading_particle > 180) {
// delta_heading_particle -= 360;
// }
// //Switch between simple and improved evaluation
// //"Simple" only evaluates the deviation between the measured heading and the particle heading change using
// //normal distribution
// //if(simple) {
// double sigma_delta_heading = sigma_heading;
// const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle);
// return p;
// // }
// return prob;
}