FHWS/Fusion2016
Archived
3
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

began putting everything together

Browse Source
This commit is contained in:
FrankE
2016-01-28 21:49:36 +01:00
parent 07d739ebb7
commit 41713a5d47
30 changed files with 1446 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files

76
code/toni/BarometerEvaluation.h
View File

@@ -2,10 +2,10 @@
#include "../particles/MyState.h"
#include "BarometerObservation.h"
#include "barometric.h"
#include <KLib/math/distribution/Normal.h>
//#include "barometric.h"
//#include <KLib/math/distribution/Normal.h>
double g_BarometerObservation = 0.0;
static constexpr double g_BarometerObservation = 0.0;
class BarometerEvaluation {
@@ -13,46 +13,50 @@ public:
double getProbability(const MyState& state, const BarometerObservation* obs) const {
//rho_z
double barometerSigma = 0.3;
return 1;
//The height of the single floor levels.
const static double floor_height[3] = {4.1, 3.4, 3.4};
// //rho_z
// double barometerSigma = 0.3;
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];
}
// //The height of the single floor levels.
// const static double floor_height[3] = {4.1, 3.4, 3.4};
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);
}
// 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];
// }
}
else {
// constant value for sigma if we assume all floors are same in height
barometerSigma = 0.30 / 1.0; //hPa
}
// 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);
// }
// evaluate the current particle with a normal distribution
const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma/2, obs->hpa);
// }
// else {
// // constant value for sigma if we assume all floors are same in height
// barometerSigma = 0.30 / 1.0; //hPa
// }
//Just for the visualization. i'm a lazy bastard
g_BarometerObservation = obs->hpa;
// // evaluate the current particle with a normal distribution
// const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma/2, obs->hpa);
assert(barometerProbability == barometerProbability);
assert(state.hPa == state.hPa);
assert(obs->hpa == obs->hpa);
// //Just for the visualization. i'm a lazy bastard
// g_BarometerObservation = obs->hpa;
//std::cout << barometerProbability << std::endl;
// 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;
}
return pow(2.0, barometerProbability);
//return barometerProbability;
}
};

2
code/toni/BarometerSensorReader.h
View File

@@ -2,7 +2,7 @@
#include "circular.h"
#include "BarometerObservation.h"
#include "../SensorReader.h"
#include "../reader/SensorReader.h"
#include <sstream>
//circular_buffer<double> measurementHistory(1000);