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

updated sensors and filter to current code version

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

84
nav/mesh/NavControllerMesh.cpp
View File

@@ -31,37 +31,35 @@
Q_DECLARE_METATYPE(const void*)
/** ctor */
MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh) :
NavController(mainController, im), navMesh(navMesh), wifiModel(im) {
MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel) :
NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel) {
// filter init
std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new MeshBased::PFInit(navMesh));
std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh));
// // estimation
// //std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationWeightedAverage<MyState>());
// std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
// estimation
std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MeshBased::MyState>> estimation(new SMC::ParticleFilterEstimationWeightedAverage<MeshBased::MyState>());
//std::unique_ptr<SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
// // resampling
// std::unique_ptr<NodeResampling<MyState, MyGridNode>> resample(new NodeResampling<MyState, MyGridNode>(*grid));
// //std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>> resample(new K::ParticleFilterResamplingSimple<MyState>());
// //std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>> resample(new K::ParticleFilterResamplingPercent<MyState>(0.05));
// //std::unique_ptr<RegionalResampling> resample(new RegionalResampling());
// resampling
//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<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>());
// // eval and transition
// wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
// std::unique_ptr<K::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel));
// std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid));
// eval and transition
std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new MeshBased::PFEval(wifiModel));
std::unique_ptr<SMC::ParticleFilterTransition<MyState, MyControl>> transition(new MeshBased::PFTrans(navMesh));
// // setup the filter
// pf = std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>>(new K::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));
// 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->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);
@@ -70,7 +68,7 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
SensorFactory::get().getWiFi().addListener(this);
SensorFactory::get().getSteps().addListener(this);
SensorFactory::get().getTurns().addListener(this);
//SensorFactory::get().getActivity().addListener(this);
SensorFactory::get().getActivity().addListener(this);
// hacky.. but we need to call this one from the main thread!
//mainController->getMapView()->showParticles(pf->getParticles());
@@ -161,14 +159,14 @@ void MeshBased::NavControllerMesh::onSensorData(Sensor<TurnData>* sensor, const
gotSensorData(ts);
}
// void NavControllerMesh::onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) {
// (void) sensor;
// (void) ts;
// curCtrl.activity = data.curActivity;
// curObs.activity = data.curActivity;
// debugActivity(data.curActivity);
// gotSensorData(ts);
// }
void MeshBased::NavControllerMesh::onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) {
(void) sensor;
(void) ts;
curCtrl.activity = data.curActivity;
curObs.activity = data.curActivity;
//debugActivity(data.curActivity);
gotSensorData(ts);
}
/** called when any sensor has received new data */
void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
@@ -176,16 +174,16 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();}
}
// void debugActivity(const ActivityData& activity) {
// QString act;
// switch(activity.curActivity) {
// case ActivityButterPressure::Activity::STAY: act = "STAY"; break;
// case ActivityButterPressure::Activity::DOWN: act = "DOWN"; break;
// case ActivityButterPressure::Activity::UP: act = "UP"; break;
// default: act = "???"; break;
// }
// Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed");
// }
// void debugActivity(const ActivityData& activity) {
// QString act;
// switch(activity.curActivity) {
// case Activity::STANDING: act = "STAY"; break;
// case Activity::WALKING_DOWN: act = "DOWN"; break;
// case Activity::WALKING_UP: act = "UP"; break;
// default: act = "???"; break;
// }
// Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed");
// }
/** particle-filter update loop */
void MeshBased::NavControllerMesh::filterUpdateLoop() {