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

performance enhancements

Browse Source 
better 2D drawing
This commit is contained in:
FrankE
2016-09-29 21:02:19 +02:00
parent 5ddc455bee
commit 833327bafd
10 changed files with 185 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
nav/Filter.h
View File

@@ -26,6 +26,7 @@
#include "../Settings.h"
#include <omp.h>
#include <future>
class PFInit : public K::ParticleFilterInitializer<MyState> {
@@ -113,25 +114,19 @@ public:
((MyControl*)_ctrl)->resetAfterTransition();
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
double probSum = 0;
// seems OK
// float sum = 0;
// for (int i = 0; i < 1000; ++i) {
// float val = noise(gen);
// sum += std::abs(val);
// }
//Log::add("123", "sum: " + std::to_string(sum));
//Log::add("123", std::to_string(Timestamp::fromRunningTime().ms()) + ": " + std::to_string(ctrl.numStepsSinceLastTransition));
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
//for (K::Particle<MyState>& p : particles) {
#pragma omp parallel for num_threads(2)
for (int i = 0; i < (int) particles.size(); ++i) {
K::Particle<MyState>& p = particles[i];
#pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) {
//#pragma omp atomic
const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
K::Particle<MyState>& p = particles[i];
double prob;
p.state = walker.getDestination(*grid, p.state, dist_m, prob);
//p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15);
@@ -141,17 +136,8 @@ public:
p.weight = std::pow(p.weight, 0.1); // make all particles a little more equal [less strict]
p.weight *= std::pow(prob, 0.1); // add grid-walk-probability
if (p.weight != p.weight) {throw Exception("nan");}
probSum += prob;
//p.weight = Distribution::Exponential<double>::getProbability(5.0, prob);
}
// const double avgProb = probSum / particles.size();
// const double threshold = avgProb * 0.15;
// for (int i = 0; i < (int) particles.size(); ++i) {
// K::Particle<MyState>& p = particles[i];
// p.weight = (p.weight > threshold) ? (1.0) : (0.01); // downvote all transitions below the threshold
// //p.weight = 1;
// }
}
}
@@ -221,7 +207,13 @@ public:
Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2));
for (K::Particle<MyState>& p : particles) {
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
#pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) {
K::Particle<MyState>& p = particles[i];
// WiFi free
//const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, vg.group(observation.wifi));
@@ -238,10 +230,11 @@ public:
p.weight *= prob; // NOTE: keeps the weight returned by the transition step!
//p.weight = prob; // does NOT keep the weights returned by the transition step
sum += p.weight;
if (p.weight != p.weight) {throw Exception("nan");}
#pragma omp atomic
sum += p.weight;
}
return sum;