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
150 lines
4.2 KiB
C++
150 lines
4.2 KiB
C++
#ifndef FILTER_H
|
|
#define FILTER_H
|
|
|
|
#include <KLib/math/filter/particles/ParticleFilter.h>
|
|
|
|
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
|
|
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
|
|
|
|
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
|
|
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
|
|
|
|
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
|
|
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
|
|
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
|
|
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleButterActivity.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
|
|
|
|
#include "State.h"
|
|
#include "Node.h"
|
|
|
|
#include "../Settings.h"
|
|
|
|
class PFInit : public K::ParticleFilterInitializer<MyState> {
|
|
|
|
private:
|
|
|
|
Grid<MyGridNode>* grid;
|
|
|
|
public:
|
|
|
|
PFInit(Grid<MyGridNode>* grid) : grid(grid) {
|
|
|
|
}
|
|
|
|
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
|
|
|
|
std::minstd_rand gen;
|
|
std::uniform_int_distribution<int> distIdx(0, grid->getNumNodes()-1);
|
|
std::uniform_real_distribution<float> distHead(0, 2*M_PI);
|
|
|
|
for (K::Particle<MyState>& p : particles) {
|
|
const int idx = distIdx(gen);
|
|
const MyGridNode& node = (*grid)[idx];
|
|
p.state.position = node; // random position
|
|
p.state.heading.direction = Heading(distHead(gen)); // random heading
|
|
}
|
|
|
|
// // fix position + heading
|
|
// for (K::Particle<MyState>& p : particles) {
|
|
// const int idx = 9000;
|
|
// const MyGridNode& node = (*grid)[idx];
|
|
// p.state.position = node;
|
|
// p.state.heading.direction = Heading(0);
|
|
// }
|
|
|
|
}
|
|
|
|
};
|
|
|
|
class PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
|
|
|
|
public:
|
|
|
|
Grid<MyGridNode>* grid;
|
|
GridWalker<MyGridNode, MyState> walker;
|
|
|
|
WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
|
|
WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading;
|
|
WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
|
|
WalkModuleButterActivity<MyGridNode, MyState> modBarometer;
|
|
WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
|
|
|
|
std::minstd_rand gen;
|
|
|
|
public:
|
|
|
|
PFTrans(Grid<MyGridNode>* grid, MyControl* ctrl) : grid(grid), modHeading(ctrl, Settings::turnSigma), modDestination(*grid) {
|
|
|
|
walker.addModule(&modFavorZ);
|
|
walker.addModule(&modHeading);
|
|
walker.addModule(&modImportance);
|
|
walker.addModule(&modBarometer);
|
|
walker.addModule(&modDestination);
|
|
|
|
if (Settings::destination != GridPoint(0,0,0)) {
|
|
modDestination.setDestination(grid->getNodeFor(Settings::destination));
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
|
|
|
|
std::normal_distribution<float> noise(0, Settings::stepSigma);
|
|
|
|
for (K::Particle<MyState>& p : particles) {
|
|
const float dist_m = std::abs(control->numStepsSinceLastTransition * Settings::stepLength + noise(gen));
|
|
p.state = walker.getDestination(*grid, p.state, dist_m);
|
|
}
|
|
|
|
((MyControl*)control)->resetAfterTransition();
|
|
|
|
}
|
|
|
|
};
|
|
|
|
class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
|
|
|
|
WiFiModelLogDistCeiling& wifiModel;
|
|
WiFiObserverFree wiFiProbability;
|
|
|
|
public:
|
|
|
|
PFEval(WiFiModelLogDistCeiling& wifiModel) : wifiModel(wifiModel), wiFiProbability(Settings::wifiSigma, wifiModel) {
|
|
|
|
}
|
|
|
|
double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override {
|
|
|
|
double sum = 0;
|
|
|
|
// smartphone is 1.3 meter above ground
|
|
const Point3 person(0,0,Settings::smartphoneAboveGround);
|
|
|
|
// local copy!! observation might be changed async outside!! (will really produces crashes!)
|
|
const MyObservation observation = _observation;
|
|
|
|
for (K::Particle<MyState>& p : particles) {
|
|
const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, observation.wifi);
|
|
const double pGPS = 1;
|
|
const double prob = pWiFi * pGPS;
|
|
p.weight = prob;
|
|
sum += prob;
|
|
}
|
|
|
|
return sum;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
#endif // FILTER_H
|