started to add ble functions
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
@@ -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;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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();
|
||||
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user