075d8bb633
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
69 lines
1.8 KiB
C++
69 lines
1.8 KiB
C++
#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
|