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

updated sensors and filter to current code version

Browse Source 
removed KLib stuff
added new activity
filter is uncommand!
at the moment, the app is not able to load new maps and breaks using old maps
This commit is contained in:
toni
2018-07-12 18:39:27 +02:00
parent b4a1a3d969
commit 625f5fe04d
22 changed files with 325 additions and 261 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
nav/grid/Filter.h
View File

@@ -1,13 +1,18 @@
#ifndef FILTER_H
#define FILTER_H
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <Indoor/smc/Particle.h>
#include <Indoor/smc/filtering/ParticleFilter.h>
#include <Indoor/smc/filtering/ParticleFilterInitializer.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKLD.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
@@ -31,7 +36,7 @@
namespace GridBased {
class PFInit : public K::ParticleFilterInitializer<MyState> {
class PFInit : public SMC::ParticleFilterInitializer<MyState> {
private:
@@ -43,14 +48,14 @@ namespace GridBased {
}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
virtual void initialize(std::vector<SMC::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) {
for (SMC::Particle<MyState>& p : particles) {
const int idx = distIdx(gen);
const MyGridNode& node = (*grid)[idx];
p.state.position = node; // random position
@@ -59,7 +64,7 @@ namespace GridBased {
}
// // fix position + heading
// for (K::Particle<MyState>& p : particles) {
// for (SMC::Particle<MyState>& p : particles) {
//// const int idx = 9000;
//// const MyGridNode& node = (*grid)[idx];
// const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap
@@ -71,7 +76,7 @@ namespace GridBased {
};
class PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
public:
@@ -110,7 +115,7 @@ namespace GridBased {
void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* _ctrl) override {
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* _ctrl) override {
// local copy!! observation might be changed async outside!! (will really produces crashes!)
this->ctrl = *_ctrl;
@@ -121,14 +126,14 @@ namespace GridBased {
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
//for (K::Particle<MyState>& p : particles) {
//for (SMC::Particle<MyState>& p : particles) {
#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];
SMC::Particle<MyState>& p = particles[i];
double prob;
p.state = walker.getDestination(*grid, p.state, dist_m, prob);
@@ -147,7 +152,7 @@ namespace GridBased {
};
class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
Grid<MyGridNode>* grid;
@@ -170,7 +175,7 @@ namespace GridBased {
}
double getStairProb(const K::Particle<MyState>& p, const Activity act) {
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
const float kappa = 0.75;
@@ -195,7 +200,7 @@ namespace GridBased {
}
double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override {
double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {
double sum = 0;
@@ -215,7 +220,7 @@ namespace GridBased {
#pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) {
K::Particle<MyState>& p = particles[i];
SMC::Particle<MyState>& p = particles[i];
// WiFi free
//const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, vg.group(observation.wifi));

29
nav/grid/NavControllerGrid.cpp
View File

@@ -21,25 +21,25 @@ Q_DECLARE_METATYPE(const void*)
GridBased::NavControllerGrid::NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid<MyGridNode>* grid) : NavController(mainController, im), grid(grid), wifiModel(im) {
// filter init
std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
std::unique_ptr<SMC::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
// estimation
//std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationWeightedAverage<MyState>());
std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
// estimation
//std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new SMC::ParticleFilterEstimationWeightedAverage<MyState>());
std::unique_ptr<SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
// resampling
std::unique_ptr<NodeResampling<MyState, MyGridNode>> resample(new NodeResampling<MyState, MyGridNode>(*grid));
//std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>> resample(new K::ParticleFilterResamplingSimple<MyState>());
//std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>> resample(new K::ParticleFilterResamplingPercent<MyState>(0.05));
//std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>> resample(new SMC::ParticleFilterResamplingSimple<MyState>());
//std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>> resample(new SMC::ParticleFilterResamplingPercent<MyState>(0.05));
//std::unique_ptr<RegionalResampling> resample(new RegionalResampling());
// eval and transition
wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
std::unique_ptr<K::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel));
std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid));
std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel));
std::unique_ptr<SMC::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid));
// setup the filter
pf = std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>>(new K::ParticleFilter<MyState, MyControl, MyObservation>(Settings::numParticles, std::move(init)));
pf = std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>>(new SMC::ParticleFilter<MyState, MyControl, MyObservation>(Settings::numParticles, std::move(init)));
pf->setTransition(std::move(transition));
pf->setEvaluation(std::move(eval));
pf->setEstimation(std::move(estimation));
@@ -56,7 +56,7 @@ GridBased::NavControllerGrid::NavControllerGrid(Controller* mainController, Floo
SensorFactory::get().getWiFi().addListener(this);
SensorFactory::get().getSteps().addListener(this);
SensorFactory::get().getTurns().addListener(this);
//SensorFactory::get().getActivity().addListener(this);
SensorFactory::get().getActivity().addListener(this);
}
@@ -141,6 +141,15 @@ void GridBased::NavControllerGrid::onSensorData(Sensor<TurnData>* sensor, const
gotSensorData(ts);
}
void GridBased::NavControllerGrid::onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) {
(void) sensor;
(void) ts;
curCtrl.activity = data.curActivity;
curObs.activity = data.curActivity;
//debugActivity(data.curActivity);
gotSensorData(ts);
}
/** called when any sensor has received new data */
void GridBased::NavControllerGrid::gotSensorData(const Timestamp ts) {
curObs.currentTime = ts;

4
nav/grid/NavControllerGrid.h
View File

@@ -28,7 +28,7 @@ namespace GridBased {
Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling wifiModel;
std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>> pf;
std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf;
DijkstraPath<MyGridNode> pathToDest;
@@ -60,7 +60,7 @@ namespace GridBased {
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override;
// void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ;
void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ;
private:

12
nav/grid/NodeResampling.h
View File

@@ -6,7 +6,7 @@
#include <random>
#include <Indoor/grid/Grid.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResampling.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResampling.h>
/**
@@ -16,12 +16,12 @@
* O(log(n)) per particle
*/
template <typename State, typename Node>
class NodeResampling : public K::ParticleFilterResampling<State> {
class NodeResampling : public SMC::ParticleFilterResampling<State> {
private:
/** this is a copy of the particle-set to draw from it */
std::vector<K::Particle<State>> particlesCopy;
std::vector<SMC::Particle<State>> particlesCopy;
/** random number generator */
std::minstd_rand gen;
@@ -35,7 +35,7 @@
gen.seed(1234);
}
void resample(std::vector<K::Particle<State>>& particles) override {
void resample(std::vector<SMC::Particle<State>>& particles) override {
// compile-time sanity checks
// TODO: this solution requires EXPLICIT overloading which is bad...
@@ -99,7 +99,7 @@
private:
/** draw one particle according to its weight from the copy vector */
const K::Particle<State>& draw(const double cumWeight) {
const SMC::Particle<State>& draw(const double cumWeight) {
// generate random values between [0:cumWeight]
std::uniform_real_distribution<float> dist(0, cumWeight);
@@ -108,7 +108,7 @@
const float rand = dist(gen);
// search comparator (cumWeight is ordered -> use binary search)
auto comp = [] (const K::Particle<State>& s, const float d) {return s.weight < d;};
auto comp = [] (const SMC::Particle<State>& s, const float d) {return s.weight < d;};
auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp);
return *it;

20
nav/grid/RegionalResampling.h
View File

@@ -1,12 +1,12 @@
#ifndef REGIONALRESAMPLING_H
#define REGIONALRESAMPLING_H
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResampling.h>
#include "State.h"
namespace GridBased {
class RegionalResampling : public K::ParticleFilterResampling<MyState> {
class RegionalResampling : public SMC::ParticleFilterResampling<MyState> {
public:
@@ -14,25 +14,25 @@ namespace GridBased {
RegionalResampling() {;}
void resample(std::vector<K::Particle<MyState>>& particles) override {
void resample(std::vector<SMC::Particle<MyState>>& particles) override {
Point3 sum;
for (const K::Particle<MyState>& p : particles) {
for (const SMC::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) {
std::vector<SMC::Particle<MyState>> next;
for (const SMC::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;
std::vector<SMC::Particle<MyState>> copy = particles;
double cumWeight = 0;
for ( K::Particle<MyState>& p : copy) {
for ( SMC::Particle<MyState>& p : copy) {
cumWeight += p.weight;
p.weight = cumWeight;
}
@@ -50,7 +50,7 @@ namespace GridBased {
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) {
const SMC::Particle<MyState>& draw(std::vector<SMC::Particle<MyState>>& copy, const double cumWeight) {
// generate random values between [0:cumWeight]
std::uniform_real_distribution<float> dist(0, cumWeight);
@@ -59,7 +59,7 @@ namespace GridBased {
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 comp = [] (const SMC::Particle<MyState>& s, const float d) {return s.weight < d;};
auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp);
return *it;