FrankE
deb21fc550
added new eval using shortest-path + plotting removed compiler warnings for clean-code fixed some minor issues added new TeX code and new graphics
50 lines
980 B
C++
Executable File
50 lines
980 B
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 {
|
|
|
|
(void) state;
|
|
(void) obs;
|
|
(void) simple;
|
|
|
|
return 1;
|
|
|
|
// 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;
|
|
|
|
// // proability?
|
|
// const float sigma = 15.0;
|
|
// const double prob = K::NormalDistribution::getProbability(obs->delta_heading, sigma, delta);
|
|
|
|
// if (prob != prob) {
|
|
// throw 1;
|
|
// }
|
|
|
|
// return prob;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
#endif // TURNEVALUATION_H
|