FHWS/OTHER2017
Archived
3
0
Fork 0
Code Issues 4 Pull Requests Releases Wiki Activity

next commit

Browse Source
This commit is contained in:
FrankE
2017-04-05 18:45:00 +02:00
parent 6171582b40
commit b2adb16b49
18 changed files with 1602 additions and 360 deletions
Download Patch File Download Diff File Expand all files Collapse all files

130
pf/EvalWalk.h
View File

@@ -8,8 +8,9 @@
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingNEff.h>
#include "../PlotErrFunc.h"
#include "../plots/PlotErrFunc.h"
#include <thread>
@@ -23,6 +24,7 @@
#include "Indoor/sensors/radio/VAPGrouper.h"
#include "Indoor/sensors/imu/StepDetection.h"
#include "Indoor/sensors/imu/TurnDetection.h"
#include "Indoor/sensors/activity/ActivityDetector.h"
#include "Indoor/floorplan/v2/Floorplan.h"
#include "Indoor/floorplan/v2/FloorplanReader.h"
@@ -34,6 +36,9 @@
#include "Indoor/sensors/offline/FileReader.h"
#include "../Helper.h"
#include "PF.h"
#include "../plots/PlotErrTime.h"
#include <Indoor/debug/PlotWifiMeasurements.h>
#include <Indoor/sensors/offline/FilePlayer.h>
#include <Indoor/sensors/offline/FileReader.h>
@@ -48,6 +53,7 @@ class EvalWalk : public Offline::Listener {
WiFiModelLogDistCeiling wifiModel;
Plotty plotty;
PlotWifiMeasurements plotWifi;
Offline::FileReader reader;
Offline::FilePlayer player;
@@ -61,18 +67,24 @@ class EvalWalk : public Offline::Listener {
StepDetection stepDetect;
TurnDetection turnDetect;
ActivityDetector actDetect;
std::vector<Point3> groundTruth;
Floorplan::IndoorMap* map;
EarthMapping em;
float absHead = 0;
public:
EvalWalk(Floorplan::IndoorMap* map) : wifiModel(map), plotty(map), map(map) {
EvalWalk(Floorplan::IndoorMap* map) : wifiModel(map), plotty(map), map(map), em(map) {
const std::string saveFile = Settings::pathData + "/grid.dat";
grid = new Grid<MyGridNode>(Settings::Grid::gridSize_cm);
// once
plotty.buildFloorplan();
// deserialize grid
@@ -93,17 +105,24 @@ public:
pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)) );
// TODO: flexible model
wifiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF, false);
std::unique_ptr<PFEval> eval = std::unique_ptr<PFEval>( new PFEval(grid, wifiModel) );
pf->setEvaluation( std::move(eval) );
//wifiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF, false);
wifiModel.saveXML("/tmp/test.xml");
wifiModel.loadXML("/tmp/test.xml");
// transition
pf->setTransition( std::unique_ptr<PFTrans>( new PFTrans(grid)) );
// resampling step?
pf->setNEffThreshold(0.5);
//pf->setNEffThreshold(0.5);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.10)) );
//pf->setNEffThreshold(0.75);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.10)) );
//pf->setNEffThreshold(0.75);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.05)) );
pf->setNEffThreshold(1.0);
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingNEff<MyState>>(new K::ParticleFilterResamplingNEff<MyState>(0.50, 0.05)) );
// state estimation step
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -117,15 +136,32 @@ public:
void walk1() {
runName = "path2_forward_simple";
std::string path = Settings::path1a;
groundTruth = FloorplanHelper::getGroundTruth(map, Settings::GroundTruth::path1);
// path1
absHead = M_PI/2;
const std::string path = Settings::path1b;
const std::vector<int> pathPoints = Settings::GroundTruth::path1;
//GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
pf->setTransition( std::unique_ptr<PFTrans>( new PFTrans(grid)) );
// path2
// absHead = 0;
// const std::string path = Settings::path2a;
// const std::vector<int> pathPoints = Settings::GroundTruth::path2;
runName = "";
// get ground-truth
groundTruth = FloorplanHelper::getGroundTruth(map, pathPoints);
// wifi model
WiFiModelLogDistCeiling wifiModel(map);
wifiModel.loadXML(Settings::wifiEachOptParPos);
// eval
std::unique_ptr<PFEval> eval = std::unique_ptr<PFEval>( new PFEval(grid, wifiModel, em) );
pf->setEvaluation( std::move(eval) );
// data-file
reader.open(path);
groundTruthLive = reader.getGroundTruth(map, Settings::GroundTruth::path1);
groundTruthLive = reader.getGroundTruth(map, pathPoints);
player.setReader(&reader);
player.setListener(this);
player.start();
@@ -152,6 +188,7 @@ public:
++curCtrl.numStepsSinceLastTransition;
}
gotSensorData(ts);
curCtrl.activityNew = actDetect.add(ts, data);
}
virtual void onGravity(const Timestamp ts, const GravityData data) override {
@@ -161,6 +198,8 @@ public:
virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) override {
std::cout << "WIFI" << std::endl;
curObs.wifi = data;
plotWifi.add(Settings::WiFiModel::vg_eval.group(data));
plotWifi.plot();
}
virtual void onBarometer(const Timestamp ts, const BarometerData data) override {
@@ -225,17 +264,31 @@ private:
K::Statistics<float> statsErr;
int updateCount = 0;
int getNumFHWSAPs(const WiFiMeasurements& mes) {
std::unordered_set<std::string> set;
for (const WiFiMeasurement& m : mes.entries) {
std::string mac = m.getAP().getMAC().asString();
mac.back() = '0';
set.insert(mac);
}
return set.size();
}
/** perform a filter-update (called from a background-loop) */
void filterUpdate() {
++updateCount;
static PlotErrTime pet("\\small{time (sec)}", "\\small{error (m)}", "\\small{APs visible}");
static PlotErrFunc pef("\\small{error (m)}", "\\small{updates (\\%)}");
pef.showMarkers(true);
std::cout << "update" << std::endl;
MyControl ctrlCopy = curCtrl;
static float absHead = M_PI/2; absHead += ctrlCopy.turnSinceLastTransition_rad;
//static float absHead = relHeadingOffset;
absHead += ctrlCopy.turnSinceLastTransition_rad;
//lastEst = curEst;
curEst = pf->update(&curCtrl, curObs);
@@ -245,25 +298,41 @@ private:
plotty.setCurEst(curEst.position.inMeter());
plotty.setGroundTruth(curGT);
// error between ground-truth and estimation
const float estRealErr = curEst.position.inMeter().getDistance(curGT);
statsErr.add(estRealErr);
pef.clear();
pef.add("", statsErr);
pef.plot();
if (updateCount > 4) {
// error between ground-truth and estimation
const float estRealErr = curEst.position.inMeter().getDistance(curGT);
statsErr.add(estRealErr);
pef.clear();
pef.add("", &statsErr);
pef.plot();
// timed error
pet.addErr(lastTransition, estRealErr);
pet.addB(lastTransition, getNumFHWSAPs(curObs.wifi));
pet.plot();
// update estimated path
const K::GnuplotPoint3 p3(curEst.position.x_cm, curEst.position.y_cm, curEst.position.z_cm);
plotty.pathEst.add(p3/100);
}
std::cout << statsErr.asString() << std::endl;
const K::GnuplotPoint3 p3(curEst.position.x_cm, curEst.position.y_cm, curEst.position.z_cm);
plotty.pathEst.add(p3/100);
// show particles
float maxWeight = 0;
float minWeight = 99;
plotty.particles.clear();
for (const auto p : pf->getParticles()) {
const K::GnuplotPoint3 p3(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
plotty.particles.add(p3/100);
plotty.particles.add(p3/100, p.weight);
if (p.weight > maxWeight) {maxWeight = p.weight;}
if (p.weight < minWeight) {minWeight = p.weight;}
}
plotty.gp << "set cbrange [" << minWeight << ":" << maxWeight << "] \n";
// GT
// show ground-truth
plotty.pathReal.clear();
for (const Point3 pt : groundTruth) {
plotty.pathReal.add(K::GnuplotPoint3(pt.x, pt.y, pt.z));
@@ -272,9 +341,12 @@ private:
std::string title =
" time " + std::to_string(curObs.currentTime.sec()) +
" steps: " + std::to_string(ctrlCopy.numStepsSinceLastTransition) +
" turn: " + std::to_string(ctrlCopy.turnSinceLastTransition_rad);
" turn: " + std::to_string(ctrlCopy.turnSinceLastTransition_rad) +
" APs: " + std::to_string(curObs.wifi.entries.size()) +
" Act: " + std::to_string((int)curCtrl.activityNew);
plotty.setTitle(title);
// relative heading and compass
{
Point2 cen(0.1, 0.9);
Point2 dir(std::cos(absHead), std::sin(absHead));
@@ -289,7 +361,7 @@ private:
// plot
plotty.plot();
std::this_thread::sleep_for(std::chrono::milliseconds(30));
std::this_thread::sleep_for(std::chrono::milliseconds(5));
curCtrl.resetAfterTransition();

40
pf/PF.h
View File

@@ -14,6 +14,8 @@
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
#include <Indoor/sensors/gps/GPSData.h>
#include <Indoor/sensors/gps/GPSProbability.h>
#include <Indoor/sensors/activity/Activity.h>
#include <Indoor/grid/walk/v2/GridWalker.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
@@ -113,7 +115,9 @@ struct MyControl {
// TODO: switch to a general activity enum/detector using barometer + accelerometer?
/** currently detected activity */
ActivityButterPressure::Activity activity;
ActivityButterPressure::Activity activity = ActivityButterPressure::Activity::STAY;
Activity activityNew;
/** reset the control-data after each transition */
void resetAfterTransition() {
@@ -180,10 +184,10 @@ public:
walker.addModule(&modRelHead);
walker.addModule(&modAbsHead);
walker.addModule(&modActivity);
//walker.addModule(&modFavorZ);
//walker.addModule(&modImportance);
//walker.addModule(&modActivity);
// if (Settings::destination != GridPoint(0,0,0)) {
// //walker.addModule(&modDestination);
@@ -215,6 +219,8 @@ public:
K::Particle<MyState>& p = particles[i];
p.weight = std::pow(p.weight, 0.8);
double prob;
p.state = walker.getDestination(*grid, p.state, dist_m, prob);
//p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15);
@@ -223,11 +229,16 @@ public:
//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
p.weight *= prob; // grid-walk-probability
if (p.weight != p.weight) {throw Exception("nan");}
}
// normalize
double weightSum = 0;
for (const auto& p : particles) {weightSum += p.weight;}
for (auto& p : particles) {p.weight /= weightSum;}
}
};
@@ -238,6 +249,8 @@ class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
WiFiModelLogDistCeiling& wifiModel;
EarthMapping& em;
WiFiObserverFree wiFiProbability; // free-calculation
//WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation
@@ -247,8 +260,8 @@ class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
public:
PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel) :
grid(grid), wifiModel(wifiModel),
PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel, EarthMapping& em) :
grid(grid), wifiModel(wifiModel), em(em),
wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free
//wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid
@@ -280,6 +293,7 @@ public:
}
double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override {
double sum = 0;
@@ -292,6 +306,9 @@ public:
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi);
const int numAP2 = wifiObs.entries.size();
// GPS
const GPSProbability gpsProb(em);
Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2));
// sanity check
@@ -301,6 +318,8 @@ public:
for (int i = 0; i < Settings::numParticles; ++i) {
K::Particle<MyState>& p = particles[i];
const MyGridNode& node = grid->getNodeFor(p.state.position);
// WiFi free
const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, wifiObs);
@@ -309,12 +328,13 @@ public:
//const MyGridNode& node = grid->getNodeFor(p.state.position);
//const double pWiFi = wiFiProbability.getProbability(node, observation.currentTime, wifiObs);
//const double pStair = 1;//getStairProb(p, observation.activity);
const double pGPS = gpsProb.getProbability(p.state.position.inMeter(), observation.gps);
const double prob = pWiFi * pGPS;
//Log::add("xxx", std::to_string(observation.currentTime.ms()) + "_" + std::to_string(wifiObs.entries[0].ts.ms()));
const double pStair = 1;//getStairProb(p, observation.activity);
const double pGPS = 1;
const double prob = pWiFi * pGPS * pStair;
if (pGPS != 1) {
int i = 0; (void) i;
}
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