59 lines
1.9 KiB
C++
Executable File
59 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>
|
|
|
|
double g_BarometerObservation = 0.0;
|
|
|
|
class BarometerEvaluation {
|
|
|
|
public:
|
|
|
|
double getProbability(const MyState& state, const BarometerObservation* obs) const {
|
|
|
|
//rho_z
|
|
double barometerSigma = 0.3;
|
|
|
|
//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/2, 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;
|
|
}
|
|
};
|