FrankE
c5a5acbbf6
started working on eval-graphics ned helper methods tested some new aspects some fixes and changes added some graphics new test-floorplan many cleanups
73 lines
1.7 KiB
C++
Executable File
73 lines
1.7 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 constexpr double sigma_heading = 5;
|
|
|
|
class TurnEvaluation {
|
|
|
|
//All calculations use degree not rad!!!
|
|
|
|
public:
|
|
|
|
double getProbability(const MyState& state, const TurnObservation* obs, bool simple = false) const {
|
|
|
|
return 1;
|
|
|
|
// 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;
|
|
|
|
// proability?
|
|
const float sigma = 15.0;
|
|
const double prob = K::NormalDistribution::getProbability(obs->delta_heading, sigma, delta);
|
|
|
|
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;
|
|
// // }
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
#endif // TURNEVALUATION_H
|