77 lines
2.1 KiB
C++
Executable File
77 lines
2.1 KiB
C++
Executable File
#ifndef TURNEVALUATION_H
|
|
#define TURNEVALUATION_H
|
|
|
|
|
|
#include "../particles/MyState.h"
|
|
#include "TurnObservation.h"
|
|
#include <boost/math/special_functions/bessel.hpp>
|
|
#include <math.h>
|
|
|
|
static double sigma_heading = 35;
|
|
|
|
class TurnEvaluation {
|
|
|
|
//All calculations use degree not rad!!!
|
|
|
|
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;
|
|
|
|
|
|
//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;
|
|
}
|
|
|
|
//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;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
#endif // TURNEVALUATION_H
|