52 lines
1.1 KiB
C++
Executable File
52 lines
1.1 KiB
C++
Executable File
|
|
#ifndef STEPEVALUATION_H
|
|
#define STEPEVALUATION_H
|
|
|
|
|
|
|
|
#include "../particles/MyState.h"
|
|
#include "StepObservation.h"
|
|
#include <math.h>
|
|
|
|
static double mu_walk = 40;
|
|
static double sigma_walk = 15;
|
|
static double mu_stop = 0;
|
|
static double sigma_stop = 5;
|
|
|
|
class StepEvaluation {
|
|
|
|
public:
|
|
|
|
double getProbability(const MyState& state, const StepObservation* obs) const {
|
|
|
|
double distance = state.distanceWalkedCM;
|
|
|
|
double a = 1.0;
|
|
double mu_distance = 0; //cm
|
|
double sigma_distance = 10.0; //cm
|
|
|
|
if(obs->step) {
|
|
a = 1.0;
|
|
mu_distance = mu_walk;//80.0; //cm
|
|
sigma_distance = sigma_walk;//40.0; //cm
|
|
}
|
|
|
|
else {
|
|
a = 0.0;
|
|
mu_distance = mu_stop; //cm
|
|
sigma_distance = sigma_stop; //cm
|
|
}
|
|
|
|
//Mixed Gaussian model: 1st Gaussian = step, 2nd Gaussian = no step
|
|
const double p = a * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance) +
|
|
(1.0-a) * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance);
|
|
|
|
return p;
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
#endif // STEPEVALUATION_H
|