FrankE
716b004f3c
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
53 lines
1012 B
C++
Executable File
53 lines
1012 B
C++
Executable File
|
|
#ifndef STEPEVALUATION_H
|
|
#define STEPEVALUATION_H
|
|
|
|
|
|
|
|
#include "../particles/MyState.h"
|
|
#include "StepObservation.h"
|
|
#include <math.h>
|
|
|
|
static constexpr double mu_walk = 90;
|
|
static constexpr double sigma_walk = 30;
|
|
static constexpr double mu_stop = 0;
|
|
static constexpr double sigma_stop = 5;
|
|
|
|
class StepEvaluation {
|
|
|
|
public:
|
|
|
|
double getProbability(const MyState& state, const StepObservation* obs) const {
|
|
|
|
return 1;
|
|
|
|
// see: particle-filter-control-data
|
|
// const float mdlWalked_m = state.walkState.distanceWalked_m;
|
|
|
|
// ((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;
|
|
|
|
// 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;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
#endif // STEPEVALUATION_H
|