a lot!!! of changes
added main menu added debug display many debug widgets for plotting live data worked on android live sensors added offline-data sensor feeding some dummy data sensors worked on the map display added ui debug for grid-points, particles and weights added a cool dude to display the estimation added real filtering based on the Indoor components c++11 fixes for android compilation online and offline filtering support new resampling technique for testing map loading via dialog
This commit is contained in:
68
nav/RegionalResampling.h
Normal file
68
nav/RegionalResampling.h
Normal file
@@ -0,0 +1,68 @@
|
||||
#ifndef REGIONALRESAMPLING_H
|
||||
#define REGIONALRESAMPLING_H
|
||||
|
||||
#include <KLib/math/filter/particles/ParticleFilter.h>
|
||||
#include "State.h"
|
||||
|
||||
class RegionalResampling : public K::ParticleFilterResampling<MyState> {
|
||||
|
||||
public:
|
||||
|
||||
float maxDist = 12.5;
|
||||
|
||||
RegionalResampling() {;}
|
||||
|
||||
void resample(std::vector<K::Particle<MyState>>& particles) override {
|
||||
|
||||
Point3 sum;
|
||||
for (const K::Particle<MyState>& p : particles) {
|
||||
sum += p.state.position.inMeter();
|
||||
}
|
||||
const Point3 avg = sum / particles.size();
|
||||
|
||||
std::vector<K::Particle<MyState>> next;
|
||||
for (const K::Particle<MyState>& p : particles) {
|
||||
const float dist = p.state.position.inMeter().getDistance(avg);
|
||||
if (rand() % 6 != 0) {continue;}
|
||||
if (dist < maxDist) {next.push_back(p);}
|
||||
}
|
||||
|
||||
// cumulate
|
||||
std::vector<K::Particle<MyState>> copy = particles;
|
||||
double cumWeight = 0;
|
||||
for ( K::Particle<MyState>& p : copy) {
|
||||
cumWeight += p.weight;
|
||||
p.weight = cumWeight;
|
||||
}
|
||||
|
||||
// draw missing particles
|
||||
const int missing = particles.size() - next.size();
|
||||
for (int i = 0; i < missing; ++i) {
|
||||
next.push_back(draw(copy, cumWeight));
|
||||
}
|
||||
|
||||
std::swap(next, particles);
|
||||
|
||||
}
|
||||
|
||||
std::minstd_rand gen;
|
||||
|
||||
/** draw one particle according to its weight from the copy vector */
|
||||
const K::Particle<MyState>& draw(std::vector<K::Particle<MyState>>& copy, const double cumWeight) {
|
||||
|
||||
// generate random values between [0:cumWeight]
|
||||
std::uniform_real_distribution<float> dist(0, cumWeight);
|
||||
|
||||
// draw a random value between [0:cumWeight]
|
||||
const float rand = dist(gen);
|
||||
|
||||
// search comparator (cumWeight is ordered -> use binary search)
|
||||
auto comp = [] (const K::Particle<MyState>& s, const float d) {return s.weight < d;};
|
||||
auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp);
|
||||
return *it;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // REGIONALRESAMPLING_H
|
||||
Reference in New Issue
Block a user