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

current state

Browse Source
This commit is contained in:
FrankE
2017-04-18 18:03:31 +02:00
parent 3e244118cc
commit 11cb939758
18 changed files with 1809 additions and 516 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
pf/EvalWalk.h
View File

@@ -108,7 +108,7 @@ public:
pf->setTransition( std::unique_ptr<PFTrans>( new PFTrans(grid)) );
// resampling step?
//pf->setNEffThreshold(0.35);
//pf->setNEffThreshold(0.15);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
//pf->setNEffThreshold(0.75);
@@ -117,22 +117,26 @@ public:
//pf->setNEffThreshold(0.75);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.05)) );
K::ParticleFilterResamplingNEff<MyState>* res = new K::ParticleFilterResamplingNEff<MyState>(0.50, 0.05);
K::ParticleFilterResamplingNEff<MyState>* res = new K::ParticleFilterResamplingNEff<MyState>(0.75, 0.05);
pf->setNEffThreshold(1.0);
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingNEff<MyState>>(res) );
res->setDrawCallback([&] (K::Particle<MyState>& p) {
static std::minstd_rand gen;
const MyGridNode* n = grid->getNodePtrFor(p.state.position);
std::normal_distribution<float> distTurn(-0.3, +0.3);
for (int j = 0; j < 2; ++j) {
std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
const int idx = distIdx(gen);
n = &(grid->getNeighbor(*n, idx));
}
p.state.position = *n;
p.state.heading.direction += distTurn(gen);
});
// move during resampling. NOT ALLOWED!
// res->setDrawCallback([&] (K::Particle<MyState>& p) {
// static std::minstd_rand gen;
// static int cnt = 0; ++cnt;
// bool init = cnt < pf->getParticles().size() * 50;
// const MyGridNode* n = grid->getNodePtrFor(p.state.position);
// std::normal_distribution<float> distTurn(0, (init) ? (0.5) : (0.05));
// for (int j = 0; j < 2; ++j) {
// std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
// const int idx = distIdx(gen);
// n = &(grid->getNeighbor(*n, idx));
// }
// p.state.position = *n;
// p.state.heading.direction += distTurn(gen);
// });
// state estimation step
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -144,15 +148,20 @@ public:
void walk1() {
// path1
absHead = M_PI/2;
const std::string path = Settings::path1b;
const std::vector<int> pathPoints = Settings::GroundTruth::path1;
// absHead = M_PI/2;
// const std::string path = Settings::path1b;
// const std::vector<int> pathPoints = Settings::GroundTruth::path1;
// path2
// absHead = 0;
// const std::string path = Settings::path2a;
// const std::string path = Settings::path2b;
// const std::vector<int> pathPoints = Settings::GroundTruth::path2;
// path_toni_inst_2
absHead = M_PI;
const std::string path = Settings::path_toni_inst_2b;
const std::vector<int> pathPoints = Settings::GroundTruth::path_toni_inst_2;
runName = "";
// get ground-truth
@@ -162,8 +171,12 @@ public:
//WiFiModelLogDistCeiling wifiModel(map);
//wifiModel.loadXML(Settings::wifiAllFixed);
//wifiModel.loadXML(Settings::wifiEachOptParPos);
WiFiModelPerFloor wifiModel(map);
wifiModel.loadXML(Settings::wifiEachOptParPos_multimodel);
//WiFiModelPerFloor wifiModel(map);
//wifiModel.loadXML(Settings::wifiEachOptParPos_multimodel);
WiFiModelPerBBox wifiModel(map);
wifiModel.loadXML(Settings::wifiEachOptParPos_perBBox);
// eval
std::unique_ptr<PFEval> eval = std::unique_ptr<PFEval>( new PFEval(grid, wifiModel, em) );
@@ -226,6 +239,7 @@ public:
const float diff = Angle::getSignedDiffRAD_2PI(curCtrl.compassAzimuth_rad, newAzimuth_safe);
curCtrl.compassAzimuth_rad += diff * 0.01;
curCtrl.compassAzimuth_rad = Angle::makeSafe_2PI(curCtrl.compassAzimuth_rad);
curObs.compassAzimuth_rad = curCtrl.compassAzimuth_rad;
//curCtrl.compassAzimuth_rad = curCtrl.compassAzimuth_rad * 0.99 + newAzimuth * 0.01;
}

144
pf/PF.h
View File

@@ -12,7 +12,8 @@
#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/radio/WiFiQualityAnalyzer.h>
#include <Indoor/sensors/gps/GPSData.h>
#include <Indoor/sensors/gps/GPSProbability.h>
#include <Indoor/sensors/activity/Activity.h>
@@ -92,6 +93,9 @@ struct MyObservation {
/** gps measurements */
GPSData gps;
/** absolute heading in radians */
float compassAzimuth_rad = 0;
// TODO: switch to a general activity enum/detector for barometer + accelerometer + ...?
/** detected activity */
ActivityButterPressure::Activity activity;
@@ -181,9 +185,8 @@ public:
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modRelHead(&ctrl, Settings::IMU::turnSigma), modAbsHead(&ctrl, Settings::IMU::absHeadSigma), modDestination(*grid), modActivity(&ctrl) {
walker.addModule(&modRelHead);
walker.addModule(&modAbsHead);
//walker.addModule(&modAbsHead);
//walker.addModule(&modActivity);
//walker.addModule(&modFavorZ);
@@ -197,40 +200,77 @@ public:
}
int numTrans = 0;
void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* _ctrl) override {
++numTrans;
// 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> turnNoise (0, 0.2);
// std::uniform_real_distribution<float> initTurnNoise(-0.3, +0.3);
// std::uniform_real_distribution<float> initDistNoise(0.0, 0.5);
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
//for (K::Particle<MyState>& p : particles) {
//#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));
float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
K::Particle<MyState>& p = particles[i];
p.weight = std::pow(p.weight, 0.5);
// first transitions: more variation as the state is unknown
if (numTrans < 50 || numTrans % 10 == 0) {
const MyGridNode* n = grid->getNodePtrFor(p.state.position);
std::normal_distribution<float> distTurn(0, 0.4);
for (int j = 0; j < 5; ++j) {
std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
const int idx = distIdx(gen);
n = &(grid->getNeighbor(*n, idx));
}
p.state.position = *n;
if (numTrans < 50) {
p.state.heading.direction += distTurn(gen);
dist_m += 0.5;
}
}
//p.state.heading.direction += turnNoise(gen);
// "blur" the density by adjusting the weights
//p.weight = std::pow(p.weight, 0.4);
//p.weight = 1;
double prob;
double prob = 1.0;
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.7); // add grid-walk-probability
p.weight *= prob; // grid-walk-probability
if (p.weight != p.weight) {throw Exception("nan");}
if (p.weight != p.weight) {
throw Exception("nan");
}
if (p.weight == 0) {
std::cout << "one particle weight is 0.0!" << std::endl;
//throw Exception("weight = 0.0");
}
}
@@ -253,7 +293,6 @@ class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
WiFiObserverFree wiFiProbability; // free-calculation
//WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation
// smartphone is 1.3 meter above ground
const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);
@@ -264,88 +303,109 @@ public:
grid(grid), wifiModel(wifiModel), em(em),
wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free
//wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid
}
double getStairProb(const K::Particle<MyState>& p, const ActivityButterPressure::Activity act) {
const float kappa = 0.75;
double getAbsHead(const MyState& s, const MyObservation& obs) {
const MyGridNode& gn = grid->getNodeFor(p.state.position);
switch (act) {
// compare the heading against the state's heading - the last error
const Heading stateHead = s.heading.direction;
const Heading obsHead(obs.compassAzimuth_rad);
case ActivityButterPressure::Activity::STAY:
if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;}
if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;}
{return 1-kappa;}
// get the difference
const float angularDiff = obsHead.getDiffHalfRAD(stateHead);
case ActivityButterPressure::Activity::UP:
case ActivityButterPressure::Activity::DOWN:
if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;}
if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
{return 1-kappa;}
double res = 0;
}
if (angularDiff > Angle::degToRad(90)) {res = 0.05;}
else if (angularDiff > Angle::degToRad(45)) {res = 0.25;}
else {res = 0.70;}
return 1.0;
return res;
}
WiFiQualityAnalyzer wqa;
double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override {
double sum = 0;
// 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);
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));
wqa.add(wifiObs);
const float quality = wqa.getQuality();
// GPS
const GPSProbability gpsProb(em);
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!");
// use (0.9 * p) + (0.1 * (1-p)) for error cases
wiFiProbability.setUseError(false);
//#pragma omp parallel for num_threads(3)
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);
double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, wifiObs);
double pWiFiMod = Distribution::Exponential<double>::getProbability(0.20, -std::log(pWiFi));
double pWiFiVeto = wiFiProbability.getVeto(p.state.position.inMeter()+person, observation.currentTime, wifiObs);
if (quality < 0.2) {
pWiFi = 1;
pWiFiVeto = 1;
std::cout << "disabling WiFi" << std::endl;
}
// WiFi grid
//const MyGridNode& node = grid->getNodeFor(p.state.position);
//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;
const double pAbsHead = getAbsHead(p.state, observation);
const double prob = pWiFi * pWiFiVeto;// * pAbsHead;
//GPS ERROR?!?!?! does it work without disabling wifi when gps is disabled?
if (pGPS != 1) {
int i = 0; (void) i;
}
// TESTING
//p.weight = std::pow(p.weight, 0.5);
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;
if (p.weight != p.weight) {
throw Exception("nan");
}
if (p.weight == 0) {
std::cout << "one particle weight is 0.0!" << std::endl;
// throw Exception("weight = 0.0");
}
}
return sum;
return 0;
}