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

started to add ble functions

Browse Source 
added ble as sensor to java and c++
added sensorlistener for ble
added ble to observation and onDataSensor in filter
started to work on ble fingerprints for optimization
This commit is contained in:
mail@toni-fetzer.de
2019-06-05 18:04:31 +02:00
parent b9b9d8f9ac
commit 20ae2f5c2a
23 changed files with 1341 additions and 1152 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
nav/NavController.h
View File

@@ -42,66 +42,67 @@
class Controller;
class NavController :
public SensorListener<AccelerometerData>,
public SensorListener<GyroscopeData>,
public SensorListener<BarometerData>,
public SensorListener<WiFiMeasurements>,
public SensorListener<GPSData>,
public SensorListener<StepData>,
public SensorListener<AccelerometerData>,
public SensorListener<BeaconMeasurement>,
public SensorListener<GyroscopeData>,
public SensorListener<BarometerData>,
public SensorListener<WiFiMeasurements>,
public SensorListener<GPSData>,
public SensorListener<StepData>,
public SensorListener<TurnData>,
public SensorListener<ActivityData>
{
{
protected:
Controller* mainController;
Floorplan::IndoorMap* im;
Controller* mainController;
Floorplan::IndoorMap* im;
bool running = false;
std::thread tFilter;
std::thread tDisplay;
bool running = false;
std::thread tFilter;
std::thread tDisplay;
/** the estimated path */
std::vector<Point3> estPath;
/** the estimated path */
std::vector<Point3> estPath;
/** all listeners */
std::vector<NavControllerListener*> listeners;
/** all listeners */
std::vector<NavControllerListener*> listeners;
/** display stuff */
const int display_ms = Settings::MapView3D::msPerFrame.ms();
Point3 curPosFast;
Point3 curPosSlow;
CurEst curEst;
/** display stuff */
const int display_ms = Settings::MapView3D::msPerFrame.ms();
Point3 curPosFast;
Point3 curPosSlow;
CurEst curEst;
Timestamp lastTransition;
Timestamp lastTransition;
public:
NavController(Controller* mainController, Floorplan::IndoorMap* im);
NavController(Controller* mainController, Floorplan::IndoorMap* im);
virtual ~NavController() {
if (running) {stop();}
}
virtual ~NavController() {
if (running) {stop();}
}
/** attach a new event listener */
void addListener(NavControllerListener* l) {
listeners.push_back(l);
}
/** attach a new event listener */
void addListener(NavControllerListener* l) {
listeners.push_back(l);
}
public:
virtual void stop() {;}
virtual void stop() {;}
virtual void start() = 0;
virtual void start() = 0;
int cameraMode = 0;
void toggleCamera() {
cameraMode = (cameraMode + 1) % 3;
}
int cameraMode = 0;
void toggleCamera() {
cameraMode = (cameraMode + 1) % 3;
}
/** update the map-view (called from within a background-loop) */
void updateMapView();
/** update the map-view (called from within a background-loop) */
void updateMapView();
};

46
nav/Observation.h
View File

@@ -2,45 +2,49 @@
#define STATE_H
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/sensors/beacon/BeaconMeasurements.h>
#include <Indoor/sensors/gps/GPSData.h>
#include <Indoor/sensors/activity/Activity.h>
/** observed sensor data */
struct MyObservation {
/** wifi measurements */
WiFiMeasurements wifi;
/** wifi measurements */
WiFiMeasurements wifi;
/** gps measurements */
GPSData gps;
/** ble measurements */
BeaconMeasurements ble;
// TODO: switch to a general activity enum/detector for barometer + accelerometer + ...?
/** detected activity */
Activity activity;
/** gps measurements */
GPSData gps;
/** time of evaluation */
Timestamp currentTime;
// TODO: switch to a general activity enum/detector for barometer + accelerometer + ...?
/** detected activity */
Activity activity;
/** time of evaluation */
Timestamp currentTime;
};
/** (observed) control data */
struct MyControl {
/** turn angle (in radians) since the last transition */
float turnSinceLastTransition_rad = 0;
/** turn angle (in radians) since the last transition */
float turnSinceLastTransition_rad = 0;
/** number of steps since the last transition */
int numStepsSinceLastTransition = 0;
/** number of steps since the last transition */
int numStepsSinceLastTransition = 0;
// TODO: switch to a general activity enum/detector using barometer + accelerometer?
/** currently detected activity */
Activity activity;
// TODO: switch to a general activity enum/detector using barometer + accelerometer?
/** currently detected activity */
Activity activity;
/** reset the control-data after each transition */
void resetAfterTransition() {
turnSinceLastTransition_rad = 0;
numStepsSinceLastTransition = 0;
}
/** reset the control-data after each transition */
void resetAfterTransition() {
turnSinceLastTransition_rad = 0;
numStepsSinceLastTransition = 0;
}
};

255
nav/grid/Filter.h
View File

@@ -31,6 +31,7 @@
#include "NodeResampling.h"
#include "../Settings.h"
#include <../misc/fixc11.h>
#include <omp.h>
#include <future>
@@ -38,218 +39,218 @@ namespace GridBased {
class PFInit : public SMC::ParticleFilterInitializer<MyState> {
private:
private:
Grid<MyGridNode>* grid;
Grid<MyGridNode>* grid;
public:
public:
PFInit(Grid<MyGridNode>* grid) : grid(grid) {
PFInit(Grid<MyGridNode>* grid) : grid(grid) {
}
}
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);
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 (SMC::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
p.weight = 1.0 / particles.size(); // equal weight
}
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
p.weight = 1.0 / particles.size(); // equal weight
}
// // fix position + heading
// // fix position + heading
// 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
// p.state.position = node;
// p.state.heading.direction = Heading(0);
// }
//// const int idx = 9000;
//// const MyGridNode& node = (*grid)[idx];
// const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap
// p.state.position = node;
// p.state.heading.direction = Heading(0);
// }
}
}
};
};
class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
public:
public:
/** local, static control-data COPY */
MyControl ctrl;
/** local, static control-data COPY */
MyControl ctrl;
Grid<MyGridNode>* grid;
GridWalker<MyGridNode, MyState> walker;
Grid<MyGridNode>* grid;
GridWalker<MyGridNode, MyState> walker;
WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading;
WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading;
WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
NodeResampling<MyState, MyGridNode> resampler;
NodeResampling<MyState, MyGridNode> resampler;
std::minstd_rand gen;
std::minstd_rand gen;
public:
public:
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl), resampler(*grid) {
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl), resampler(*grid) {
//walker.addModule(&modFavorZ);
walker.addModule(&modHeading);
//walker.addModule(&modImportance);
walker.addModule(&modActivity);
//walker.addModule(&modFavorZ);
walker.addModule(&modHeading);
//walker.addModule(&modImportance);
walker.addModule(&modActivity);
if (Settings::destination != GridPoint(0,0,0)) {
//walker.addModule(&modDestination);
modDestination.setDestination(grid->getNodeFor(Settings::destination));
}
if (Settings::destination != GridPoint(0,0,0)) {
//walker.addModule(&modDestination);
modDestination.setDestination(grid->getNodeFor(Settings::destination));
}
}
}
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;
((MyControl*)_ctrl)->resetAfterTransition();
// local copy!! observation might be changed async outside!! (will really produces crashes!)
this->ctrl = *_ctrl;
((MyControl*)_ctrl)->resetAfterTransition();
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
//for (SMC::Particle<MyState>& p : particles) {
#pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++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));
//#pragma omp atomic
const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
SMC::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);
//p.weight = (prob > 0.01) ? (1.0) : (0.15);
//p.weight = prob;
//p.weight = 1.0; // reset
//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
p.weight = prob; // grid-walk-probability
if (p.weight != p.weight) {throw Exception("nan");}
double prob;
p.state = walker.getDestination(*grid, p.state, dist_m, prob);
//p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15);
//p.weight = (prob > 0.01) ? (1.0) : (0.15);
//p.weight = prob;
//p.weight = 1.0; // reset
//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
p.weight = prob; // grid-walk-probability
if (p.weight != p.weight) {throw Exception("nan");}
}
}
}
}
};
};
class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
Grid<MyGridNode>* grid;
Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling& wifiModel;
WiFiModelLogDistCeiling& wifiModel;
//WiFiObserverFree wiFiProbability; // free-calculation
WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation
//WiFiObserverFree wiFiProbability; // free-calculation
WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation
// smartphone is 1.3 meter above ground
const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);
// smartphone is 1.3 meter above ground
const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);
public:
public:
PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel) :
grid(grid), wifiModel(wifiModel),
//wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free
wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid
PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel) :
grid(grid), wifiModel(wifiModel),
//wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free
wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid
}
}
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
const float kappa = 0.75;
const float kappa = 0.75;
const MyGridNode& gn = grid->getNodeFor(p.state.position);
switch (act) {
const MyGridNode& gn = grid->getNodeFor(p.state.position);
switch (act) {
case Activity::STANDING:
case Activity::WALKING:
if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;}
if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;}
{return 1-kappa;}
case Activity::STANDING:
case Activity::WALKING:
if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;}
if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;}
{return 1-kappa;}
case Activity::WALKING_UP:
case Activity::WALKING_DOWN:
if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;}
if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
{return 1-kappa;}
case Activity::WALKING_UP:
case Activity::WALKING_DOWN:
if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;}
if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
{return 1-kappa;}
}
}
return 1.0;
return 1.0;
}
}
double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {
double sum = 0;
double sum = 0;
// local copy!! observation might be changed async outside!! (will really produces crashes!)
const MyObservation observation = _observation;
// local copy!! observation might be changed async outside!! (will really produces crashes!)
const MyObservation observation = _observation;
// vap-grouping
const int numAP1 = observation.wifi.entries.size();
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi);
const int numAP2 = wifiObs.entries.size();
// vap-grouping
const int numAP1 = observation.wifi.entries.size();
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi);
const int numAP2 = wifiObs.entries.size();
Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2));
Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2));
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
// 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) {
#pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++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));
// WiFi free
//const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, vg.group(observation.wifi));
// WiFi grid
const MyGridNode& node = grid->getNodeFor(p.state.position);
const double pWiFi = wiFiProbability.getProbability(node, observation.currentTime, wifiObs);
// WiFi grid
const MyGridNode& node = grid->getNodeFor(p.state.position);
const double pWiFi = wiFiProbability.getProbability(node, observation.currentTime, wifiObs);
//Log::add("xxx", std::to_string(observation.currentTime.ms()) + "_" + std::to_string(wifiObs.entries[0].ts.ms()));
//Log::add("xxx", std::to_string(observation.currentTime.ms()) + "_" + std::to_string(wifiObs.entries[0].ts.ms()));
const double pStair = getStairProb(p, observation.activity);
const double pGPS = 1;
const double prob = pWiFi * pGPS * pStair;
const double pStair = getStairProb(p, observation.activity);
const double pGPS = 1;
const double prob = pWiFi * pGPS * pStair;
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
if (p.weight != p.weight) {throw Exception("nan");}
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
if (p.weight != p.weight) {throw Exception("nan");}
#pragma omp atomic
sum += p.weight;
#pragma omp atomic
sum += p.weight;
}
}
return sum;
return sum;
}
}
};
};
}

291
nav/grid/NavControllerGrid.cpp
View File

@@ -14,131 +14,140 @@
#include "../ui/map/2D/MapView2D.h"
#include "../ui/debug/InfoWidget.h"
#include "../Controller.h"
#include "../../Controller.h"
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
// filter init
std::unique_ptr<SMC::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
// 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));
// resampling
std::unique_ptr<NodeResampling<MyState, MyGridNode>> resample(new NodeResampling<MyState, MyGridNode>(*grid));
//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());
//std::unique_ptr<RegionalResampling> resample(new RegionalResampling());
// eval and transition
wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
// eval and transition
wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
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
// setup the filter
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));
pf->setResampling(std::move(resample));
pf->setTransition(std::move(transition));
pf->setEvaluation(std::move(eval));
pf->setEstimation(std::move(estimation));
pf->setResampling(std::move(resample));
pf->setNEffThreshold(0.85); //before 0.75, edit by toni
//pf->setNEffThreshold(0.65); // still too low?
//pf->setNEffThreshold(0.25); // too low
pf->setNEffThreshold(0.85); //before 0.75, edit by toni
//pf->setNEffThreshold(0.65); // still too low?
//pf->setNEffThreshold(0.25); // too low
// attach as listener to all sensors
SensorFactory::get().getAccelerometer().addListener(this);
SensorFactory::get().getGyroscope().addListener(this);
SensorFactory::get().getBarometer().addListener(this);
SensorFactory::get().getWiFi().addListener(this);
SensorFactory::get().getSteps().addListener(this);
SensorFactory::get().getTurns().addListener(this);
// attach as listener to all sensors
SensorFactory::get().getAccelerometer().addListener(this);
SensorFactory::get().getGyroscope().addListener(this);
SensorFactory::get().getBarometer().addListener(this);
SensorFactory::get().getWiFi().addListener(this);
SensorFactory::get().getSteps().addListener(this);
SensorFactory::get().getTurns().addListener(this);
SensorFactory::get().getActivity().addListener(this);
SensorFactory::get().getBLE().addListener(this);
}
void GridBased::NavControllerGrid::start() {
Assert::isFalse(running, "already started!");
running = true;
curCtrl.resetAfterTransition(); // ensure we start empty ;)
tFilter = std::thread(&NavControllerGrid::filterUpdateLoop, this);
tDisplay = std::thread(&NavControllerGrid::updateMapViewLoop, this);
Assert::isFalse(running, "already started!");
running = true;
curCtrl.resetAfterTransition(); // ensure we start empty ;)
tFilter = std::thread(&NavControllerGrid::filterUpdateLoop, this);
tDisplay = std::thread(&NavControllerGrid::updateMapViewLoop, this);
// start all sensors
SensorFactory::get().getAccelerometer().start();
SensorFactory::get().getGyroscope().start();
SensorFactory::get().getBarometer().start();
SensorFactory::get().getWiFi().start();
// start all sensors
SensorFactory::get().getAccelerometer().start();
SensorFactory::get().getGyroscope().start();
SensorFactory::get().getBarometer().start();
SensorFactory::get().getWiFi().start();
SensorFactory::get().getBLE().start();
#ifndef ANDROID
// #include <valgrind/callgrind.h>
// run with
// valgrind --tool=callgrind --quiet --instr-atstart=no ./yasmin
// show with
// kcachegrind callgrind.out.xxxx
CALLGRIND_START_INSTRUMENTATION;
// #include <valgrind/callgrind.h>
// run with
// valgrind --tool=callgrind --quiet --instr-atstart=no ./yasmin
// show with
// kcachegrind callgrind.out.xxxx
CALLGRIND_START_INSTRUMENTATION;
#endif
}
void GridBased::NavControllerGrid::stop() {
Assert::isTrue(running, "not started!");
running = false;
tFilter.join();
tDisplay.join();
Assert::isTrue(running, "not started!");
running = false;
tFilter.join();
tDisplay.join();
}
void GridBased::NavControllerGrid::onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) {
(void) sensor;
(void) data;
(void) ts;
gotSensorData(ts);
(void) sensor;
(void) data;
(void) ts;
gotSensorData(ts);
}
void GridBased::NavControllerGrid::onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) {
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
}
void GridBased::NavControllerGrid::onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) {
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
}
void GridBased::NavControllerGrid::onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) {
(void) sensor;
(void) ts;
curObs.wifi = data;
gotSensorData(ts);
(void) sensor;
(void) ts;
curObs.wifi = data;
gotSensorData(ts);
}
void GridBased::NavControllerGrid::onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) {
(void) sensor;
(void) ts;
curObs.gps = data;
gotSensorData(ts);
(void) sensor;
(void) ts;
curObs.gps = data;
gotSensorData(ts);
}
void GridBased::NavControllerGrid::onSensorData(Sensor<BeaconMeasurement>* sensor, const Timestamp ts, const BeaconMeasurement& data) {
(void) sensor;
(void) ts;
//Log::add("Beacon", "MAC: " + data.getBeacon().getMAC().asString() + "RSSI: " + std::to_string(data.getRSSI()));
gotSensorData(ts);
}
void GridBased::NavControllerGrid::onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) {
(void) sensor;
(void) ts;
curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
(void) sensor;
(void) ts;
curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
}
void GridBased::NavControllerGrid::onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) {
(void) sensor;
(void) ts;
curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
(void) sensor;
(void) ts;
curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
}
void GridBased::NavControllerGrid::onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) {
@@ -152,8 +161,8 @@ void GridBased::NavControllerGrid::onSensorData(Sensor<ActivityData>* sensor, co
/** called when any sensor has received new data */
void GridBased::NavControllerGrid::gotSensorData(const Timestamp ts) {
curObs.currentTime = ts;
if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();}
curObs.currentTime = ts;
if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();}
}
// void debugActivity(const ActivityData& activity) {
@@ -167,11 +176,11 @@ void GridBased::NavControllerGrid::gotSensorData(const Timestamp ts) {
// Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed");
// }
/** particle-filter update loop */
void GridBased::NavControllerGrid::filterUpdateLoop() {
/** particle-filter update loop */
void GridBased::NavControllerGrid::filterUpdateLoop() {
while(running && !Settings::Filter::useMainThread) {
while(running && !Settings::Filter::useMainThread) {
// // fixed update rate based on the systems time -> LIVE! even for offline data
// const Timestamp ts1 = Timestamp::fromUnixTime();
@@ -181,95 +190,95 @@ void GridBased::NavControllerGrid::gotSensorData(const Timestamp ts) {
// const Timestamp sleep = Timestamp::fromMS(500) - needed;
// std::this_thread::sleep_for(std::chrono::milliseconds(sleep.ms()));
const bool wasUpdated = filterUpdateIfNeeded();
if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); }
const bool wasUpdated = filterUpdateIfNeeded();
if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); }
}
}
}
}
/** check whether its time for a filter update, and if so, execute the update and return true */
bool GridBased::NavControllerGrid::filterUpdateIfNeeded() {
/** check whether its time for a filter update, and if so, execute the update and return true */
bool GridBased::NavControllerGrid::filterUpdateIfNeeded() {
static float avgSum = 0;
static int avgCount = 0;
static float avgSum = 0;
static int avgCount = 0;
// fixed update rate based on incoming sensor data
// allows working with live data and faster for offline data
const Timestamp diff = curObs.currentTime - lastTransition;
if (diff >= Settings::Filter::updateEvery) {
// fixed update rate based on incoming sensor data
// allows working with live data and faster for offline data
const Timestamp diff = curObs.currentTime - lastTransition;
if (diff >= Settings::Filter::updateEvery) {
// as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one
const Timestamp err = diff - Settings::Filter::updateEvery;
lastTransition = curObs.currentTime - err;
// as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one
const Timestamp err = diff - Settings::Filter::updateEvery;
lastTransition = curObs.currentTime - err;
const Timestamp ts1 = Timestamp::fromUnixTime();
filterUpdate();
const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1;
const QString filterTime = QString::number(tsDiff.ms());
avgSum += tsDiff.ms(); ++avgCount;
//Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount));
QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime));
return true;
const Timestamp ts1 = Timestamp::fromUnixTime();
filterUpdate();
const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1;
const QString filterTime = QString::number(tsDiff.ms());
avgSum += tsDiff.ms(); ++avgCount;
//Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount));
QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime));
return true;
} else {
} else {
return false;
return false;
}
}
}
}
/** perform a filter-update (called from a background-loop) */
void GridBased::NavControllerGrid::filterUpdate() {
/** perform a filter-update (called from a background-loop) */
void GridBased::NavControllerGrid::filterUpdate() {
//lastEst = curEst;
MyState sCurEst = pf->update(&curCtrl, curObs);
curEst.pos_m = sCurEst.position.inMeter();
curEst.head = sCurEst.heading.direction;
//Log::add("Nav", "cur est: " + curEst.position.asString());
//lastEst = curEst;
MyState sCurEst = pf->update(&curCtrl, curObs);
curEst.pos_m = sCurEst.position.inMeter();
curEst.head = sCurEst.heading.direction;
//Log::add("Nav", "cur est: " + curEst.position.asString());
// inform listeners about the new estimation
for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.pos_m);}
// inform listeners about the new estimation
for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.pos_m);}
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
// update estimated path
estPath.push_back(curEst.pos_m);
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
// update estimated path
estPath.push_back(curEst.pos_m);
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
PFTrans* trans = (PFTrans*)pf->getTransition();
const MyGridNode* node = grid->getNodePtrFor(sCurEst.position);
if (node) {
try {
pathToDest = trans->modDestination.getShortestPath(*node);
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
} catch (...) {;}
}
// mainController->getMapView()->showGridImportance();
PFTrans* trans = (PFTrans*)pf->getTransition();
const MyGridNode* node = grid->getNodePtrFor(sCurEst.position);
if (node) {
try {
pathToDest = trans->modDestination.getShortestPath(*node);
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
} catch (...) {;}
}
// mainController->getMapView()->showGridImportance();
}
}
/** UI update loop */
void GridBased::NavControllerGrid::updateMapViewLoop() {
/** UI update loop */
void GridBased::NavControllerGrid::updateMapViewLoop() {
while(running) {
const Timestamp ts1 = Timestamp::fromUnixTime();
updateMapView();
const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1;
const QString mapViewTime = QString::number(tsDiff.ms());
//QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime));
std::this_thread::sleep_for(std::chrono::milliseconds(display_ms));
}
}
while(running) {
const Timestamp ts1 = Timestamp::fromUnixTime();
updateMapView();
const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1;
const QString mapViewTime = QString::number(tsDiff.ms());
//QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime));
std::this_thread::sleep_for(std::chrono::milliseconds(display_ms));
}
}

64
nav/grid/NavControllerGrid.h
View File

@@ -21,68 +21,70 @@
namespace GridBased {
class NavControllerGrid : public NavController {
class NavControllerGrid : public NavController {
private:
private:
Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling wifiModel;
Grid<MyGridNode>* grid;
WiFiModelLogDistCeiling wifiModel;
std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf;
DijkstraPath<MyGridNode> pathToDest;
DijkstraPath<MyGridNode> pathToDest;
MyObservation curObs;
MyControl curCtrl;
MyObservation curObs;
MyControl curCtrl;
public:
public:
NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid<MyGridNode>* grid);
NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid<MyGridNode>* grid);
void start() override;
void start() override;
void stop() override;
void stop() override;
void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override;
void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override;
void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override;
void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override;
void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override;
void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override;
void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override;
void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override;
void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override;
void onSensorData(Sensor<BeaconMeasurement>* sensor, const Timestamp ts, const BeaconMeasurement& data) override;
void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override ;
void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override;
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override;
void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override ;
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override;
void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ;
private:
private:
/** called when any sensor has received new data */
void gotSensorData(const Timestamp ts);
/** called when any sensor has received new data */
void gotSensorData(const Timestamp ts);
// void debugActivity(const ActivityData& activity);
// void debugActivity(const ActivityData& activity);
/** particle-filter update loop */
void filterUpdateLoop();
/** particle-filter update loop */
void filterUpdateLoop();
/** check whether its time for a filter update, and if so, execute the update and return true */
bool filterUpdateIfNeeded();
/** check whether its time for a filter update, and if so, execute the update and return true */
bool filterUpdateIfNeeded();
/** perform a filter-update (called from a background-loop) */
void filterUpdate();
/** perform a filter-update (called from a background-loop) */
void filterUpdate();
/** UI update loop */
void updateMapViewLoop();
/** UI update loop */
void updateMapViewLoop();
};
};
}

176
nav/mesh/FilterMesh.h
View File

@@ -32,33 +32,33 @@
namespace MeshBased {
class PFInit : public SMC::ParticleFilterInitializer<MyState> {
class PFInit : public SMC::ParticleFilterInitializer<MyState> {
private:
const NM::NavMesh<NM::NavMeshTriangle>* mesh;
const NM::NavMesh<NM::NavMeshTriangle>* mesh;
public:
PFInit(const NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) {
;
PFInit(const NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) {
;
}
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
std::minstd_rand gen;
std::uniform_real_distribution<float> distHead(0, 2*M_PI);
NM::NavMeshRandom<NM::NavMeshTriangle> rnd = mesh->getRandom();
for (SMC::Particle<MyState>& p : particles) {
p.state.loc = rnd.draw();
for (SMC::Particle<MyState>& p : particles) {
p.state.pos = rnd.draw();
p.state.heading = Heading(distHead(gen)); // random heading
p.weight = 1.0 / particles.size(); // equal weight
}
// // fix position + heading
// for (SMC::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,109 +71,109 @@ namespace MeshBased {
};
class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
public:
//using MyNavMeshWalk = NM::NavMeshWalkSimple<NM::NavMeshTriangle>;
using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<NM::NavMeshTriangle>;
MyNavMeshWalk walker;
//using MyNavMeshWalk = NM::NavMeshWalkSimple<NM::NavMeshTriangle>;
using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<NM::NavMeshTriangle>;
MyNavMeshWalk walker;
// local, static control-data COPY
// local, static control-data COPY
MyControl ctrl;
public:
PFTrans(NM::NavMesh<NM::NavMeshTriangle>* mesh) : walker(*mesh){
PFTrans(NM::NavMesh<NM::NavMeshTriangle>* mesh) : walker(*mesh){
// how to evaluate drawn points
walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<NM::NavMeshTriangle>(0.04));
walker.addEvaluator(new NM::WalkEvalDistance<NM::NavMeshTriangle>(0.1));
//walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
// how to evaluate drawn points
walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<NM::NavMeshTriangle>(0.04));
walker.addEvaluator(new NM::WalkEvalDistance<NM::NavMeshTriangle>(0.1));
//walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
}
void transition(std::vector<SMC::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;
((MyControl*)_ctrl)->resetAfterTransition();
// walking and heading random
Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
Distribution::Normal<float> dHeading(0.0, 0.1);
// walking and heading random
Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
Distribution::Normal<float> dHeading(0.0, 0.1);
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
// how to walk
NM::NavMeshWalkParams<NM::NavMeshTriangle> params;
params.heading = p.state.heading + ctrl.turnSinceLastTransition_rad + dHeading.draw();
params.numSteps = ctrl.numStepsSinceLastTransition;
params.start = p.state.loc;
// how to walk
NM::NavMeshWalkParams<NM::NavMeshTriangle> params;
params.heading = p.state.heading + ctrl.turnSinceLastTransition_rad + dHeading.draw();
params.numSteps = ctrl.numStepsSinceLastTransition;
params.start = p.state.pos;
params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
if(params.stepSizes.stepSizeFloor_m < 0.1 || params.stepSizes.stepSizeStair_m < 0.1){
params.stepSizes.stepSizeFloor_m = 0.1;
params.stepSizes.stepSizeStair_m = 0.1;
}
if(params.stepSizes.stepSizeFloor_m < 0.1 || params.stepSizes.stepSizeStair_m < 0.1){
params.stepSizes.stepSizeFloor_m = 0.1;
params.stepSizes.stepSizeStair_m = 0.1;
}
// walk
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
// walk
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
// assign back to particle's state
p.weight *= res.probability;
p.state.loc = res.location;
p.state.heading = res.heading;
}
// assign back to particle's state
p.weight *= res.probability;
p.state.pos = res.location;
p.state.heading = res.heading;
}
}
};
class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
WiFiModel& wifiModel;
WiFiObserverFree wifiProbability;
WiFiModel& wifiModel;
WiFiObserverFree wifiProbability;
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
const float kappa = 0.9;
const float kappa = 0.9;
switch (act) {
switch (act) {
case Activity::WALKING:
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;}
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;}
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
{return 1-kappa;}
case Activity::WALKING:
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
{return 1-kappa;}
case Activity::WALKING_UP:
case Activity::WALKING_DOWN:
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;}
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;}
{return 1-kappa;}
}
case Activity::WALKING_UP:
case Activity::WALKING_DOWN:
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;}
{return 1-kappa;}
}
return 1.0;
}
return 1.0;
}
public:
//TODO: Was ist hier besser? Im Museum hatten wir das unterste.
PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel){}
//PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::EXPONENTIAL){}
//PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){}
//TODO: Was ist hier besser? Im Museum hatten wir das unterste.
PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel){}
//PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::EXPONENTIAL){}
//PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){}
double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {
double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {
double sum = 0;
@@ -181,32 +181,32 @@ namespace MeshBased {
const MyObservation observation = _observation;
// vap-grouping
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
// sanity check
//Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
//Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
// assign weights
#pragma omp parallel for num_threads(3)
for (size_t i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
// assign weights
#pragma omp parallel for num_threads(3)
for (size_t i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
const double pWifi = wifiProbability.getProbability(p.state.loc.pos, observation.currentTime, wifiObs);
const double pStair = getStairProb(p, observation.activity);
const double pGPS = 1;
const double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);
const double pStair = getStairProb(p, observation.activity);
const double pGPS = 1;
//TODO: reduziere das gewicht von partikelen die durch sample imp. oder was anderes sehr weit gesprungen sind.
//TODO: reduziere das gewicht von partikelen die durch sample imp. oder was anderes sehr weit gesprungen sind.
const double prob = pWifi * pStair * pGPS;
const double prob = pWifi * pStair * pGPS;
p.weight *= prob;
if (p.weight != p.weight) {throw Exception("nan");}
p.weight *= prob;
if (p.weight != p.weight) {throw Exception("nan");}
#pragma omp atomic
sum += p.weight;
}
#pragma omp atomic
sum += p.weight;
}
return sum;
return sum;
}

207
nav/mesh/NavControllerMesh.cpp
View File

@@ -23,7 +23,7 @@
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingPercent.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKDE.h>
//#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKDE.h>
//#ifndef ANDROID
//#include <valgrind/callgrind.h>
@@ -37,7 +37,7 @@ Q_DECLARE_METATYPE(const void*)
MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel) :
NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel) {
// filter init
// filter init
std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh));
// estimation
@@ -47,11 +47,11 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
// resampling
std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>> resample(new SMC::ParticleFilterResamplingSimple<MyState>());
//std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>> resample(new SMC::ParticleFilterResamplingSimple<MyState>());
//std::unique_ptr<SMC::ParticleFilterResamplingKDE<MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingKDE<MyState, NM::NavMeshTriangle>(navMesh, 0.2, Point2(1,1)));
//std::unique_ptr<SMC::ParticleFilterResamplingKLD<MyState>> resample(new SMC::ParticleFilterResamplingKLD<MyState>());
//std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>> resample(new SMC::ParticleFilterResamplingPercent<MyState>(0.95));
//std::unique_ptr<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>());
std::unique_ptr<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>());
// eval and transition
std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new MeshBased::PFEval(wifiModel));
@@ -68,35 +68,37 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
//pf->setNEffThreshold(0.65); // still too low?
//pf->setNEffThreshold(0.25); // too low
// attach as listener to all sensors
SensorFactory::get().getAccelerometer().addListener(this);
SensorFactory::get().getGyroscope().addListener(this);
SensorFactory::get().getBarometer().addListener(this);
SensorFactory::get().getWiFi().addListener(this);
SensorFactory::get().getSteps().addListener(this);
SensorFactory::get().getTurns().addListener(this);
// attach as listener to all sensors
SensorFactory::get().getAccelerometer().addListener(this);
SensorFactory::get().getGyroscope().addListener(this);
SensorFactory::get().getBarometer().addListener(this);
SensorFactory::get().getWiFi().addListener(this);
SensorFactory::get().getSteps().addListener(this);
SensorFactory::get().getTurns().addListener(this);
SensorFactory::get().getActivity().addListener(this);
SensorFactory::get().getBLE().addListener(this);
// hacky.. but we need to call this one from the main thread!
//mainController->getMapView()->showParticles(pf->getParticles());
qRegisterMetaType<const void*>();
// hacky.. but we need to call this one from the main thread!
//mainController->getMapView()->showParticles(pf->getParticles());
qRegisterMetaType<const void*>();
}
void MeshBased::NavControllerMesh::start() {
Assert::isFalse(running, "already started!");
running = true;
curCtrl.resetAfterTransition(); // ensure we start empty ;)
tFilter = std::thread(&NavControllerMesh::filterUpdateLoop, this);
tDisplay = std::thread(&NavControllerMesh::updateMapViewLoop, this);
Assert::isFalse(running, "already started!");
running = true;
curCtrl.resetAfterTransition(); // ensure we start empty ;)
tFilter = std::thread(&NavControllerMesh::filterUpdateLoop, this);
tDisplay = std::thread(&NavControllerMesh::updateMapViewLoop, this);
// start all sensors
SensorFactory::get().getAccelerometer().start();
SensorFactory::get().getGyroscope().start();
SensorFactory::get().getBarometer().start();
SensorFactory::get().getWiFi().start();
// start all sensors
SensorFactory::get().getAccelerometer().start();
SensorFactory::get().getGyroscope().start();
SensorFactory::get().getBarometer().start();
SensorFactory::get().getWiFi().start();
SensorFactory::get().getBLE().start();
//#ifndef ANDROID
// // #include <valgrind/callgrind.h>
@@ -110,60 +112,67 @@ void MeshBased::NavControllerMesh::start() {
}
void MeshBased::NavControllerMesh::stop() {
Assert::isTrue(running, "not started!");
running = false;
tFilter.join();
tDisplay.join();
Assert::isTrue(running, "not started!");
running = false;
tFilter.join();
tDisplay.join();
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) {
(void) sensor;
(void) data;
(void) ts;
gotSensorData(ts);
(void) sensor;
(void) data;
(void) ts;
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) {
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) {
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) {
(void) sensor;
(void) ts;
curObs.wifi = data;
gotSensorData(ts);
(void) sensor;
(void) ts;
curObs.wifi = data;
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<BeaconMeasurement>* sensor, const Timestamp ts, const BeaconMeasurement& data) {
(void) sensor;
(void) ts;
curObs.ble.add(data);
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) {
(void) sensor;
(void) ts;
curObs.gps = data;
gotSensorData(ts);
(void) sensor;
(void) ts;
curObs.gps = data;
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) {
(void) sensor;
(void) ts;
curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
(void) sensor;
(void) ts;
curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) {
(void) sensor;
(void) ts;
curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
(void) sensor;
(void) ts;
curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) {
@@ -177,8 +186,8 @@ void MeshBased::NavControllerMesh::onSensorData(Sensor<ActivityData>* sensor, co
/** called when any sensor has received new data */
void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
curObs.currentTime = ts;
if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();}
curObs.currentTime = ts;
if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();}
}
void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity) {
@@ -193,11 +202,11 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity)
Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed");
}
/** particle-filter update loop */
void MeshBased::NavControllerMesh::filterUpdateLoop() {
/** particle-filter update loop */
void MeshBased::NavControllerMesh::filterUpdateLoop() {
while(running && !Settings::Filter::useMainThread) {
while(running && !Settings::Filter::useMainThread) {
// // fixed update rate based on the systems time -> LIVE! even for offline data
// const Timestamp ts1 = Timestamp::fromUnixTime();
@@ -207,37 +216,37 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity)
// const Timestamp sleep = Timestamp::fromMS(500) - needed;
// std::this_thread::sleep_for(std::chrono::milliseconds(sleep.ms()));
const bool wasUpdated = filterUpdateIfNeeded();
if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); }
const bool wasUpdated = filterUpdateIfNeeded();
if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); }
}
}
}
}
Timestamp lastTransition;
Timestamp lastTransition;
/** check whether its time for a filter update, and if so, execute the update and return true */
bool MeshBased::NavControllerMesh::filterUpdateIfNeeded() {
/** check whether its time for a filter update, and if so, execute the update and return true */
bool MeshBased::NavControllerMesh::filterUpdateIfNeeded() {
static float avgSum = 0;
static int avgCount = 0;
static float avgSum = 0;
static int avgCount = 0;
const Timestamp diff = curObs.currentTime - lastTransition;
if (curCtrl.numStepsSinceLastTransition > 0){
// as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one
const Timestamp err = diff - Settings::Filter::updateEvery;
// as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one
const Timestamp err = diff - Settings::Filter::updateEvery;
lastTransition = curObs.currentTime;
const Timestamp ts1 = Timestamp::fromUnixTime();
const Timestamp ts1 = Timestamp::fromUnixTime();
filterUpdate();
const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1;
const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1;
const QString filterTime = QString::number(diff.ms());
avgSum += tsDiff.ms(); ++avgCount;
//Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount));
avgSum += tsDiff.ms(); ++avgCount;
//Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount));
QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime));
return true;
return true;
} else if(diff >= Timestamp::fromMS(1000)) {
@@ -251,19 +260,19 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity)
} else {
return false;
return false;
}
}
}
}
DijkstraPath<MyGridNode> pathToDest;
DijkstraPath<MyGridNode> pathToDest;
void MeshBased::NavControllerMesh::filterUpdateEstimationOnly() {
//lastEst = curEst;
MyState sCurEst = pf->updateEvaluationOnly(curObs);
curEst.pos_m = sCurEst.loc.pos;
curEst.pos_m = sCurEst.pos.pos;
curEst.head = sCurEst.heading;
// inform listeners about the new estimation
@@ -278,12 +287,12 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity)
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
}
/** perform a filter-update (called from a background-loop) */
void MeshBased::NavControllerMesh::filterUpdate() {
/** perform a filter-update (called from a background-loop) */
void MeshBased::NavControllerMesh::filterUpdate() {
//lastEst = curEst;
MyState sCurEst = pf->update(&curCtrl, curObs);
curEst.pos_m = sCurEst.loc.pos;
curEst.pos_m = sCurEst.pos.pos;
curEst.head = sCurEst.heading;
// inform listeners about the new estimation
@@ -309,18 +318,18 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity)
// }
// mainController->getMapView()->showGridImportance();
}
}
/** UI update loop */
void MeshBased::NavControllerMesh::updateMapViewLoop() {
/** UI update loop */
void MeshBased::NavControllerMesh::updateMapViewLoop() {
while(running) {
const Timestamp ts1 = Timestamp::fromUnixTime();
updateMapView();
const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1;
const QString mapViewTime = QString::number(tsDiff.ms());
//QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime));
std::this_thread::sleep_for(std::chrono::milliseconds(display_ms));
}
}
while(running) {
const Timestamp ts1 = Timestamp::fromUnixTime();
updateMapView();
const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1;
const QString mapViewTime = QString::number(tsDiff.ms());
//QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime));
std::this_thread::sleep_for(std::chrono::milliseconds(display_ms));
}
}

56
nav/mesh/NavControllerMesh.h
View File

@@ -24,68 +24,70 @@
namespace MeshBased {
class NavControllerMesh : public NavController {
class NavControllerMesh : public NavController {
private:
private:
NM::NavMesh<NM::NavMeshTriangle>* navMesh;
NM::NavMesh<NM::NavMeshTriangle>* navMesh;
WiFiModel* wifiModel;
std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf;
MyObservation curObs;
MyControl curCtrl;
MyObservation curObs;
MyControl curCtrl;
public:
public:
NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel);
void start() override;
void start() override;
void stop() override;
void stop() override;
void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override;
void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override;
void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override;
void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override;
void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override;
void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override;
void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override;
void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override;
void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override;
void onSensorData(Sensor<BeaconMeasurement>* sensor, const Timestamp ts, const BeaconMeasurement& data) override;
void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override;
void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override;
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override;
void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override;
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override;
void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ;
private:
private:
/** called when any sensor has received new data */
void gotSensorData(const Timestamp ts);
/** called when any sensor has received new data */
void gotSensorData(const Timestamp ts);
void debugActivity(const ActivityData& activity);
/** particle-filter update loop */
void filterUpdateLoop();
/** particle-filter update loop */
void filterUpdateLoop();
/** check whether its time for a filter update, and if so, execute the update and return true */
bool filterUpdateIfNeeded();
/** check whether its time for a filter update, and if so, execute the update and return true */
bool filterUpdateIfNeeded();
/** perform a filter-update (called from a background-loop) */
void filterUpdate();
/** perform a filter-update (called from a background-loop) */
void filterUpdate();
/** perform a filter-update only with estimation (called from a background-loop) */
void filterUpdateEstimationOnly();
/** UI update loop */
void updateMapViewLoop();
/** UI update loop */
void updateMapViewLoop();
};
};
}

74
nav/mesh/State.h
View File

@@ -9,60 +9,60 @@ namespace MeshBased {
struct MyState {
NM::NavMeshLocation<NM::NavMeshTriangle> loc;
NM::NavMeshLocation<NM::NavMeshTriangle> pos;
Heading heading;
/** ctor */
MyState() : loc(), heading(0) {
MyState() : pos(), heading(0) {
;
}
/** ctor */
MyState(NM::NavMeshLocation<NM::NavMeshTriangle> loc, Heading h) : loc(loc), heading(h) {
MyState(NM::NavMeshLocation<NM::NavMeshTriangle> loc, Heading h) : pos(loc), heading(h) {
;
}
MyState& operator += (const MyState& o) {
loc.pos += o.loc.pos;
return *this;
}
MyState& operator += (const MyState& o) {
pos.pos += o.pos.pos;
return *this;
}
MyState& operator /= (const float val) {
loc.pos /= val;
return *this;
}
MyState& operator /= (const float val) {
pos.pos /= val;
return *this;
}
MyState operator * (const float val) const {
MyState copy = *this;
copy.loc.pos = copy.loc.pos * val;
return copy;
}
MyState operator * (const float val) const {
MyState copy = *this;
copy.pos.pos = copy.pos.pos * val;
return copy;
}
float getX(){
return loc.pos.x;
}
float getX(){
return pos.pos.x;
}
float getY() {
return loc.pos.y;
}
float getY() {
return pos.pos.y;
}
float getZ() {
return loc.pos.z;
}
float getZ() {
return pos.pos.z;
}
void setPosition(Point3 pos){
loc.pos = pos;
}
void setPosition(Point3 other){
pos.pos = other;
}
float getBinValue(const int dim) const {
switch (dim) {
case 0: return this->loc.pos.x;
case 1: return this->loc.pos.y;
case 2: return this->loc.pos.z;
case 3: return this->heading.getRAD();
}
throw "cant find this value within the bin";
}
float getBinValue(const int dim) const {
switch (dim) {
case 0: return this->pos.pos.x;
case 1: return this->pos.pos.y;
case 2: return this->pos.pos.z;
case 3: return this->heading.getRAD();
}
throw "cant find this value within the bin";
}
};