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

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";
}
};