62 lines
1.9 KiB
C++
Executable File
62 lines
1.9 KiB
C++
Executable File
#pragma once
|
|
|
|
#include "../particles/MyState.h"
|
|
#include "BarometerObservation.h"
|
|
//#include "barometric.h"
|
|
//#include <KLib/math/distribution/Normal.h>
|
|
|
|
static constexpr double g_BarometerObservation = 0.0;
|
|
|
|
class BarometerEvaluation {
|
|
|
|
public:
|
|
|
|
double getProbability(const MyState& state, const BarometerObservation* obs) const {
|
|
|
|
|
|
// //rho_z
|
|
double barometerSigma = 0.06;//0.12+0.04;//0.09;
|
|
|
|
// //The height of the single floor levels.
|
|
// const static double floor_height[3] = {4.1, 3.4, 3.4};
|
|
|
|
// if(USE_BAROMETRIC_FORMULAR){
|
|
// //height the particle has climbed.
|
|
// double h_1 = 0.0;
|
|
// for(int i = std::min(state.z_nr_old, state.z_nr); i < std::max(state.z_nr_old, state.z_nr); i++){
|
|
// h_1 += floor_height[i];
|
|
// }
|
|
|
|
// if(h_1 != 0.0){
|
|
// // use the barometric formular to calculate the relative pressure
|
|
// // the calculation is done assuming sea level height at every floor.
|
|
// double mslp = BarometricFormular::s_getSeaLevelPressure();
|
|
// double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0);
|
|
// barometerSigma = std::abs(mslp - pressure);
|
|
// }
|
|
|
|
// }
|
|
// else {
|
|
// // constant value for sigma if we assume all floors are same in height
|
|
// barometerSigma = 0.30 / 1.0; //hPa
|
|
// }
|
|
|
|
// // evaluate the current particle with a normal distribution
|
|
const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma, obs->hpa);
|
|
|
|
// //Just for the visualization. i'm a lazy bastard
|
|
//g_BarometerObservation = obs->hpa;
|
|
|
|
assert(barometerProbability == barometerProbability);
|
|
assert(state.hPa == state.hPa);
|
|
assert(obs->hpa == obs->hpa);
|
|
|
|
// //std::cout << barometerProbability << std::endl;
|
|
|
|
//return pow(2.0, barometerProbability);
|
|
return barometerProbability;
|
|
|
|
}
|
|
|
|
};
|