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

started changes for implementing fast smoothing

Browse Source 
different new evaluations
small changes in transition
This commit is contained in:
toni
2017-11-11 01:41:51 +01:00
parent 232175c3dc
commit bea81eab62
5 changed files with 168 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

120
filter/Logic.h
View File

@@ -24,6 +24,8 @@
#include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
#include <Indoor/sensors/beacon/BeaconProbabilityFree.h>
#include <Indoor/sensors/activity/ActivityDetector.h>
#include <KLib/math/filter/particles/ParticleFilterMixing.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
@@ -171,7 +173,7 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
PFTrans(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma), modHeadMises(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) {
walker.addModule(&modHead);
//walker.addModule(&modHeadMises);
//walker.addModule(&modHeadMises); // fürn arsch und geht net
//walker.addModule(&modSpread); // might help in some situations! keep in mind!
//walker.addModule(&modActivity);
//walker.addModule(&modHeadUgly);
@@ -334,6 +336,110 @@ struct PFTransKLDSampling : public K::ParticleFilterTransition<MyState, MyContro
struct BFTrans : public K::BackwardFilterTransition<MyState>{
public:
/**
* ctor
* @param choice the choice to use for randomly drawing nodes
* @param fp the underlying floorplan
*/
BFTrans()
{
//nothin
}
uint64_t ts = 0;
uint64_t deltaMS = 0;
/** set the current time in millisconds */
void setCurrentTime(const uint64_t ts) {
if (this->ts == 0) {
this->ts = ts;
deltaMS = 0;
} else {
deltaMS = this->ts - ts;
this->ts = ts;
}
}
/**
* smoothing transition starting at T with t, t-1,...0
* @param particles_qt q_t (Forward Filter) p2
* @param particles_qt1 q_t+1 (Smoothed Particles from Step before) p1
*/
std::vector<std::vector<double>> transition(std::vector<K::Particle<MyState>>const& particles_qt,
std::vector<K::Particle<MyState>>const& particles_qt1,
const MyControl* control) override {
// Forward Transition von q_t nach q_t+1* with tracking of particle using an id = p(q_t+1* | q_t)
//TODO: darf ich das einfach? einfach eine neue Dichte Sampeln? Brauch ich da nicht eine "Erlaubnis" (Two-Filter Smoother kann das)
// law of total probabality auch einfach über ziehen ??
// KDE auf q_t+1 Samples = p(q_t+1 | o_1:T)
// Apply Position from Samples from q_t+1* into KDE of p(q_t+1 | o_1:T) to get p(q_t+1* | o_1:T)
// Calculate new weight w(q_(t|T)) = w(q_t) * p(q_t+1* | o_1:T) * p(q_t+1* | q_t) * normalisation
// calculate alpha(m,n) = p(q_t+1(m) | q_t(n))
// this means, predict all possible states q_t+1 with all passible states q_t
// e.g. p(q_490(1)|q_489(1));p(q_490(1)|q_489(2)) ... p(q_490(1)|q_489(N)) and
// p(q_490(1)|q_489(1)); p(q_490(2)|q_489(1)) ... p(q_490(M)|q_489(1))
std::vector<std::vector<double>> predictionProbabilities;
omp_set_dynamic(0); // Explicitly disable dynamic teams
omp_set_num_threads(7);
#pragma omp parallel for shared(predictionProbabilities)
for (int i = 0; i < particles_old.size(); ++i) {
std::vector<double> innerVector;
auto p1 = &particles_old[i];
for(int j = 0; j < particles_new.size(); ++j){
auto p2 = &particles_new[j];
const double distance_m = p2->state.position.inMeter().getDistance(p1->state.position.inMeter()) / 100.0;
//TODO Incorporated Activity - see IPIN16 MySmoothingTransitionExperimental
const double distProb = K::NormalDistribution::getProbability(Settings::Smoothing::stepLength, Settings::Smoothing::stepSigma, distance_m);
// TODO: FIX THIS CORRECTLY is the heading change similiar to the measurement?
double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.heading.direction.getRAD(), p1->state.heading.direction.getRAD());
double diffDeg = Angle::radToDeg(diffRad);
double measurementRad = Angle::makeSafe_2PI(p1->state.headingChangeMeasured_rad);
double measurementDeg = Angle::radToDeg(measurementRad);
const double headingProb = K::NormalDistribution::getProbability(measurementDeg, Settings::Smoothing::headingSigma, diffDeg);
// does the angle between two particles positions is similiar to the measurement
//double angleBetweenParticles = Angle::getDEG_360(p2->state.position.x, p2->state.position.y, p1->state.position.x, p1->state.position.y);
//check how near we are to the measurement
double diffZ = (p2->state.position.inMeter().z - p1->state.position.inMeter().z) / 100.0;
const double floorProb = K::NormalDistribution::getProbability(Settings::Smoothing::zChange, Settings::Smoothing::zSigma, diffZ);
//combine the probabilities
double prob = distProb;// * floorProb * headingProb;
innerVector.push_back(prob);
}
#pragma omp critical
predictionProbabilities.push_back(innerVector);
}
return predictionProbabilities;
}
};
struct BFTransKDESlow : public K::BackwardFilterTransition<MyState>{
public:
/**
@@ -473,20 +579,20 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
return Distribution::Normal<double>::getProbability(static_cast<double>(hPa), 0.10, static_cast<double>(observation.relativePressure));
}
double getStairProb(const K::Particle<MyState>& p, const ActivityButterPressure::Activity act) {
double getStairProb(const K::Particle<MyState>& p, const Activity act) {
const float kappa = 0.75;
const float kappa = 0.65;
const MyNode& gn = grid.getNodeFor(p.state.position);
switch (act) {
case ActivityButterPressure::Activity::STAY:
case Activity::WALKING:
if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;}
if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;}
{return 1-kappa;}
case ActivityButterPressure::Activity::UP:
case ActivityButterPressure::Activity::DOWN:
case Activity::WALKING_UP:
case Activity::WALKING_DOWN:
if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;}
if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
{return 1-kappa;}
@@ -532,7 +638,7 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
//p.weight = std::pow(p.weight, 0.5);
}
const double prob = pWifi * pBaroPressure * pStairProb;
const double prob = pWifi;// * pStairProb;
p.weight = prob;