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

fixed some minor bugs using ble on android

Browse Source 
added BLE to the evaluation of the particle filter
made first evaluation inside fhws
This commit is contained in:
mail@toni-fetzer.de
2019-06-10 16:58:24 +02:00
parent ef6066ae14
commit 7fe0f22a6e
9 changed files with 311 additions and 231 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
Controller.cpp
View File

@@ -250,10 +250,52 @@ WiFiModel* buildWiFiModelOnce(Floorplan::IndoorMap* map, const std::string& fpFi
BeaconModel* buildBLEModelOnce(Floorplan::IndoorMap* map, const std::string& fpFile, const std::string& bleModelFile){
// ask questions
const QMessageBox::StandardButton replyWiFiFP = QMessageBox::question(nullptr, "BLE", "Use Fingerprints for Bluetooth Calibration?\n\nYes: Use fingerprints and num-optimize AP-Params\nNo: Use Beacons from the map.xml (pos+params)", QMessageBox::Yes|QMessageBox::No);
const QMessageBox::StandardButton replyBLEFP = QMessageBox::question(nullptr, "BLE", "Use Fingerprints for Bluetooth Calibration?\n\nYes: Use fingerprints and num-optimize AP-Params\nNo: Use Beacons from the map.xml (pos+params)", QMessageBox::Yes|QMessageBox::No);
BeaconModel* bleModel = new BeaconModelLogDistCeiling(map);
// WiFi setup
if (replyBLEFP == QMessageBox::Yes) {
std::ifstream inp(bleModelFile, std::ifstream::binary);
if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
Log::add("Controller", "Create new BLEModel");
BLECalibrationDataModel mdl(fpFile);
Assert::isFalse(mdl.getFingerprints().empty(), "no fingerprints available!");
WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
for (const BeaconFingerprint& fp : mdl.getFingerprints()) {
opt.addFingerprint(fp); //we added a function to add bluetooth fingerprints to the wifi optimizer
}
const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.MIN_10_FPS);
for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) {
const BeaconModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf);
((BeaconModelLogDistCeiling*) bleModel)->addBeacon(ap.mac, entry);
}
((BeaconModelLogDistCeiling*) bleModel)->saveXML(bleModelFile);
} else {
Log::add("Controller", "Use existing BLEModel");
XMLDoc doc;
XMLserialize::assertOK(doc.LoadFile(bleModelFile.c_str()), doc, "error while loading file");
XMLElem* root = doc.FirstChildElement("root");
//if(bleModel){delete bleModel;}
bleModel->readFromXML(&doc, root);
}
} else {
Log::add("Controller", "Read Beacons from File");
// load all APs from the floorplan and use same TXP/EXP/WAF for all of them
((BeaconModelLogDistCeiling*)bleModel)->loadBeaconsFromMap(map, Settings::BLEModel::TXP, Settings::BLEModel::EXP, Settings::BLEModel::WAF);
Assert::isFalse(bleModel->getAllBeacons().empty(), "no AccessPoints stored within the map.xml");
}
return bleModel;
}
@@ -340,6 +382,7 @@ void Controller::loadNavMesh(QDir dir) {
QFile fWiFiFP(dir.path() + "/wifi_fp.dat");
QFile fWiFiModel(dir.path() + "/wifimodel.dat");
QFile fBLEFP(dir.path() + "/ble_fp.dat");
QFile fBLEModel(dir.path() + "/blemodel.dat");
Assert::isTrue(fMap.exists(), "map.xml missing");
@@ -347,12 +390,16 @@ void Controller::loadNavMesh(QDir dir) {
QString str = QString(fMap.readAll());
im = Floorplan::Reader::readFromString(str.toStdString());
//build the wifi model
const std::string sWiFiFP = fWiFiFP.fileName().toStdString();
const std::string sWiFiModel = fWiFiModel.fileName().toStdString();
const std::string sBLEFP = fBLEFP.fileName().toStdString();
wifiModel = buildWiFiModelOnce(im, sWiFiFP, sWiFiModel);
//build the ble model
const std::string sBLEFP = fBLEFP.fileName().toStdString();
const std::string sBLEModel = fBLEModel.fileName().toStdString();
bleModel = buildBLEModelOnce(im, sBLEFP, sBLEModel);
// create navmesh
if (navMesh) {delete navMesh; navMesh = nullptr;}
NM::NavMeshSettings settings;
@@ -362,7 +409,7 @@ void Controller::loadNavMesh(QDir dir) {
// create a new navigator
if (nav) {delete nav; nav = nullptr;}
nav = new MeshBased::NavControllerMesh(this, im, navMesh, wifiModel);
nav = new MeshBased::NavControllerMesh(this, im, navMesh, wifiModel, bleModel);
WiFiCalibrationDataModel* wifiCalib = new WiFiCalibrationDataModel(sWiFiFP);
BLECalibrationDataModel* bleCalib = new BLECalibrationDataModel(sBLEFP);

2
Controller.h
View File

@@ -22,6 +22,7 @@ class InfoWidget;
class MapView3D;
class MapView2D;
class WiFiModel;
class BeaconModel;
namespace NM {
template <typename T> class NavMesh;
@@ -80,6 +81,7 @@ private:
NavController* nav = nullptr;
Floorplan::IndoorMap* im = nullptr;
WiFiModel* wifiModel = nullptr;
BeaconModel* bleModel = nullptr;
};

10
Settings.h
View File

@@ -4,6 +4,7 @@
#include <Indoor/grid/GridPoint.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/VAPGrouper.h>
#include <Indoor/sensors/beacon/BeaconMeasurementGrouper.h>
#include <QFile>
#include <QDir>
@@ -56,7 +57,16 @@ namespace Settings {
// const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::DISABLED, VAPGrouper::Aggregation::AVERAGE);
}
namespace BLEModel {
constexpr float sigma = 8.0;
constexpr float TXP = -59;
constexpr float EXP = 2.3;
constexpr float WAF = -9.0;
const BeaconMeasurementGrouper vg_eval = BeaconMeasurementGrouper(BeaconMeasurementGrouper::Mode::DISABLED, BeaconMeasurementGrouper::Aggregation::AVERAGE);
}
namespace MapView3D {
const int maxColorPoints = 10000;

11
_android/src/BLE.java
View File

@@ -29,6 +29,8 @@ public class BLE {
/** called when a scan is completed successfully */
public static native void onScanComplete(final byte[] result);
public static int dataCounter = 0;
/**
* start bluetooth scanning in the background.
* will call onScanComplete() once a scan is available
@@ -52,7 +54,7 @@ public class BLE {
final BluetoothManager mgr = (BluetoothManager) act.getSystemService(Context.BLUETOOTH_SERVICE);
bt = mgr.getAdapter();
Log.d("ble", "BLE Mangaer Start");
Log.d("ble", "BLE Manager Start");
// create the scanner
scanner = bt.getBluetoothLeScanner();
@@ -69,7 +71,12 @@ public class BLE {
// and attach the callback
mLeScanCallback = new ScanCallback() {
@Override public void onScanResult(int callbackType, android.bluetooth.le.ScanResult result) {
//Log.d("ble", result.getDevice() + " " + result.getRssi());
if(++dataCounter > 50){
Log.d("ble", result.getDevice() + " " + result.getRssi());
dataCounter = 0;
}
final byte[] rst = serialize(result);
BLE.onScanComplete(rst);

2
_android/src/WiFi.java
View File

@@ -122,7 +122,7 @@ public class WiFi {
/** try to trigger one scan process */
private static void triggerOneScan() {
//Log.d("wifi", "triggerOneScan()");
Log.d("wifi", "triggerOneScan()");
try {
if(!manager.startScan()) {throw new RuntimeException("Cant start WiFi!");}

1
nav/NavController.h
View File

@@ -5,6 +5,7 @@
#include "../sensors/GyroscopeSensor.h"
#include "../sensors/BarometerSensor.h"
#include "../sensors/WiFiSensor.h"
#include "../sensors/BLESensor.h"
#include "../sensors/SensorFactory.h"
#include "../sensors/StepSensor.h"
#include "../sensors/TurnSensor.h"

16
nav/mesh/FilterMesh.h
View File

@@ -14,11 +14,14 @@
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
#include <Indoor/sensors/beacon/model/BeaconModel.h>
#include <Indoor/sensors/beacon/BeaconProbabilityFree.h>
#include <Indoor/sensors/beacon/BeaconMeasurementGrouper.h>
#include <Indoor/navMesh/NavMesh.h>
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include <Indoor/navMesh/walk/NavMeshWalkSinkOrSwim.h>
@@ -143,6 +146,9 @@ namespace MeshBased {
WiFiModel& wifiModel;
WiFiObserverFree wifiProbability;
BeaconModel& bleModel;
BeaconObserverFree bleProbability;
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
const float kappa = 0.9;
@@ -169,7 +175,7 @@ namespace MeshBased {
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, BeaconModel* bleModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel), bleModel(*bleModel), bleProbability(Settings::BLEModel::sigma, *bleModel){}
//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){}
@@ -182,6 +188,9 @@ namespace MeshBased {
// vap-grouping
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
const BeaconMeasurements beaconObs = Settings::BLEModel::vg_eval.group(observation.ble);
//TODO: do i need a vap grouping for beacons?
// sanity check
//Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
@@ -191,13 +200,14 @@ namespace MeshBased {
for (size_t i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
const double pBLE = bleProbability.getProbability(p.state.pos.pos, observation.currentTime, beaconObs);
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.
const double prob = pWifi * pStair * pGPS;
const double prob = pBLE * pStair * pGPS * pWifi;
p.weight *= prob;
if (p.weight != p.weight) {throw Exception("nan");}

8
nav/mesh/NavControllerMesh.cpp
View File

@@ -31,11 +31,13 @@
#include "Settings.h"
#include "../Controller.h"
Q_DECLARE_METATYPE(const void*)
/** ctor */
MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel) :
NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel) {
MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel, BeaconModel* bleModel) :
NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel), bleModel(bleModel) {
// filter init
std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh));
@@ -54,7 +56,7 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
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));
std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new MeshBased::PFEval(wifiModel, bleModel));
std::unique_ptr<SMC::ParticleFilterTransition<MyState, MyControl>> transition(new MeshBased::PFTrans(navMesh));
// setup the filter

5
nav/mesh/NavControllerMesh.h
View File

@@ -12,6 +12,7 @@
#include <Indoor/navMesh/NavMeshLocation.h>
#include <Indoor/navMesh/NavMesh.h>
#include <Indoor/sensors/beacon/model/BeaconModel.h>
#include <Indoor/Assertions.h>
#include <thread>
@@ -19,7 +20,6 @@
//#include "State.h"
#include "FilterMesh.h"
#include "../Controller.h"
#include "../NavController.h"
namespace MeshBased {
@@ -30,6 +30,7 @@ namespace MeshBased {
NM::NavMesh<NM::NavMeshTriangle>* navMesh;
WiFiModel* wifiModel;
BeaconModel* bleModel;
std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf;
@@ -38,7 +39,7 @@ namespace MeshBased {
public:
NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel);
NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel, BeaconModel* bleModel);
void start() override;