105 lines
2.4 KiB
C++
Executable File
105 lines
2.4 KiB
C++
Executable File
#ifndef MYEVALUATION_H
|
|
#define MYEVALUATION_H
|
|
|
|
#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
|
|
#include "MyObservation.h"
|
|
#include "MyState.h"
|
|
|
|
#include "../frank/WiFiEvaluation.h"
|
|
#include "../frank/BeaconEvaluation.h"
|
|
#include "../toni/BarometerEvaluation.h"
|
|
#include "../lukas/StepEvaluation.h"
|
|
#include "../lukas/TurnEvaluation.h"
|
|
|
|
#include <atomic>
|
|
|
|
class MyEvaluation : public K::ParticleFilterEvaluation<MyState, MyObservation> {
|
|
|
|
private:
|
|
|
|
WiFiEvaluation wifiEval;
|
|
BeaconEvaluation beaconEval;
|
|
BarometerEvaluation barometerEval;
|
|
StepEvaluation stepEval;
|
|
TurnEvaluation turnEval;
|
|
|
|
bool useWifi = true;
|
|
bool useStep = true;
|
|
bool useTurn = true;
|
|
bool useBaro = true;
|
|
bool useIB = true;
|
|
|
|
public:
|
|
|
|
void setUsage(bool useWifi, bool useStep, bool useTurn, bool useBaro, bool useIB) {
|
|
this->useWifi = useWifi;
|
|
this->useStep = useStep;
|
|
this->useTurn = useTurn;
|
|
this->useBaro = useBaro;
|
|
this->useIB = useIB;
|
|
}
|
|
|
|
virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& observation) override {
|
|
|
|
//if (observation.wifi) {
|
|
wifiEval.nextObservation(observation.wifi);
|
|
//}
|
|
|
|
// evalulate each particle
|
|
double sum = 0;
|
|
for (K::Particle<MyState>& p : particles) {
|
|
|
|
// std::atomic<double> sum(0);
|
|
// #pragma omp parallel for
|
|
// for (int i = 0; i < (int) particles.size(); ++i) {
|
|
// K::Particle<MyState>& p = particles[i];
|
|
|
|
double weight = 1.0;
|
|
|
|
if (useWifi) {
|
|
weight *= wifiEval.getProbability(p.state, observation);
|
|
}
|
|
|
|
if (useBaro && observation.barometer) {
|
|
weight *= barometerEval.getProbability(p.state, observation.barometer);
|
|
|
|
// set
|
|
p.state.measurement_pressure = observation.barometer->hpa;
|
|
}
|
|
|
|
if (useIB) {
|
|
weight *= beaconEval.getProbability(p.state, observation);
|
|
}
|
|
|
|
// CONTROL!
|
|
// if (useStep) {
|
|
// weight *= stepEval.getProbability(p.state, observation.step);
|
|
// }
|
|
|
|
// CONTROL!
|
|
// if (useTurn) {
|
|
// weight *= turnEval.getProbability(p.state, observation.turn, true);
|
|
|
|
// //set
|
|
// p.state.angularHeadingChange = observation.turn->delta_heading;
|
|
// }
|
|
|
|
// set and accumulate
|
|
p.weight = weight;
|
|
sum += p.weight;
|
|
|
|
}
|
|
|
|
// reset
|
|
observation.step->steps = 0;
|
|
observation.turn->delta_heading = 0;
|
|
observation.turn->delta_motion = 0;
|
|
|
|
return sum;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // MYEVALUATION_H
|