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

began putting everything together

Browse Source
This commit is contained in:
FrankE
2016-01-28 21:49:36 +01:00
parent 07d739ebb7
commit 41713a5d47
30 changed files with 1446 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
code/lukas/TurnEvaluation.h
View File

@@ -4,10 +4,10 @@
#include "../particles/MyState.h"
#include "TurnObservation.h"
#include <boost/math/special_functions/bessel.hpp>
//#include <boost/math/special_functions/bessel.hpp>
#include <math.h>
static double sigma_heading = 35;
static constexpr double sigma_heading = 35;
class TurnEvaluation {
@@ -17,55 +17,33 @@ public:
double getProbability(const MyState& state, const TurnObservation* obs, bool simple = false) const {
//Particle's heading change
double delta_heading_particle = state.heading - state.heading_old;
return 1;
// //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;
}
// //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) {
// //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;
// double sigma_delta_heading = sigma_heading;
const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle);
// const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle);
return p;
}
//use the von Mises distribution
else {
//Here some calculations must be done in rad
double delta_heading_obs_rad = obs->delta_heading * 3.14159265359 / 180.0;
double delta_motion_rad = obs -> delta_motion * 3.14159265359 / 180.0;
//Equation for estimating kappa value of von Mises distribution
//empirically estimated
double kappa = 0.0;
kappa = 5.0 / exp(2 * delta_motion_rad);
double delta_heading_particle_rad = delta_heading_particle * 3.14159265359 / 180.0;
//pdf von mises distribution (http://en.wikipedia.org/wiki/Von_Mises_distribution)
const double p = exp(kappa * cos(delta_heading_obs_rad - delta_heading_particle_rad)) / (2.0 * 3.14159265359 * boost::math::cyl_bessel_i(0, kappa));
return p;
}
// return p;
// // }
}