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

current code and TeX. code fine?!?!?!

Browse Source
This commit is contained in:
FrankE
2017-04-29 20:57:12 +02:00
parent fc72a75f57
commit 60712689cf
41 changed files with 804 additions and 234 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
pf/PF.h Normal file → Executable file
View File

@@ -98,7 +98,8 @@ struct MyObservation {
// TODO: switch to a general activity enum/detector for barometer + accelerometer + ...?
/** detected activity */
ActivityButterPressure::Activity activity;
//ActivityButterPressure::Activity activity;
Activity activityNew = Activity::STANDING;
/** time of evaluation */
Timestamp currentTime;
@@ -119,9 +120,9 @@ struct MyControl {
// TODO: switch to a general activity enum/detector using barometer + accelerometer?
/** currently detected activity */
ActivityButterPressure::Activity activity = ActivityButterPressure::Activity::STAY;
//ActivityButterPressure::Activity activity = ActivityButterPressure::Activity::STAY;
Activity activityNew;
Activity activityNew = Activity::STANDING;
/** reset the control-data after each transition */
void resetAfterTransition() {
@@ -150,8 +151,10 @@ public:
std::uniform_real_distribution<float> distHead(0, 2*M_PI);
for (K::Particle<MyState>& p : particles) {
const int idx = distIdx(gen);
const MyGridNode& node = (*grid)[idx];
again:
const int idx = distIdx(gen);
const MyGridNode& node = (*grid)[idx];
if (node.getNumNeighbors() != 8) {goto again;}
p.state.position = node; // random position
p.state.heading.direction = Heading(distHead(gen)); // random heading
p.weight = 1.0 / particles.size(); // equal weight
@@ -174,7 +177,7 @@ public:
WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
//WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modRelHead;
WalkModuleAbsoluteHeadingControl<MyGridNode, MyState, MyControl> modAbsHead;
@@ -183,7 +186,11 @@ public:
public:
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modRelHead(&ctrl, Settings::IMU::turnSigma), modAbsHead(&ctrl, Settings::IMU::absHeadSigma), modDestination(*grid), modActivity(&ctrl) {
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);
@@ -234,10 +241,9 @@ public:
K::Particle<MyState>& p = particles[i];
// first transitions: more variation as the state is unknown
if (numTrans < 50 || numTrans % 10 == 0) {
if (numTrans < 70 || numTrans % (10*1) == 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);
@@ -246,9 +252,10 @@ public:
}
p.state.position = *n;
if (numTrans < 50) {
p.state.heading.direction += distTurn(gen);
dist_m += 0.5;
if (numTrans < 70) {
std::normal_distribution<float> distTurn(0, 0.3);
p.state.heading.direction += distTurn(gen);
dist_m += 0.5;
}
}
@@ -264,6 +271,11 @@ public:
p.state = walker.getDestination(*grid, p.state, dist_m, prob);
p.weight *= prob; // grid-walk-probability
//const GridNode gn = grid->getNodeFor(p.state.position);
//const double probNode = (gn.getNumNeighbors() >= 8) ? (0.7) : (0.3);
//p.weight *= probNode;
if (p.weight != p.weight) {
throw Exception("nan");
}
@@ -320,14 +332,31 @@ public:
double res = 0;
if (angularDiff > Angle::degToRad(90)) {res = 0.05;}
else if (angularDiff > Angle::degToRad(45)) {res = 0.25;}
else {res = 0.70;}
if (angularDiff > Angle::degToRad(120)) {res = 0.30;}
else {res = 0.70;}
return res;
}
double getActivity(const MyState& s, const MyObservation& obs) {
const MyGridNode& n = grid->getNodeFor(s.position);
switch(obs.activityNew) {
case Activity::WALKING:
return (n.getType() != MyGridNode::TYPE_STAIR) ? (0.7) : (0.3);
case Activity::WALKING_DOWN:
case Activity::WALKING_UP:
return (n.getType() == MyGridNode::TYPE_STAIR) ? (0.7) : (0.3);
default:
return 1;
}
return 1.0;
}
WiFiQualityAnalyzer wqa;
double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override {
@@ -343,7 +372,9 @@ public:
wqa.add(wifiObs);
const float quality = wqa.getQuality();
float quality = wqa.getQuality();
std::cout << "wifi quality: " << quality << std::endl;
// GPS
const GPSProbability gpsProb(em);
@@ -353,7 +384,7 @@ public:
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);
wiFiProbability.setUseError(true);
//#pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) {
@@ -362,16 +393,15 @@ public:
// WiFi free
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);
//double pWiFiMod = Distribution::Exponential<double>::getProbability(0.20, -std::log(pWiFi));
//double pWiFiMod = std::pow(pWifi, 0.2);
//double pWiFiVeto = wiFiProbability.getVeto(p.state.position.inMeter()+person, observation.currentTime, wifiObs);
const bool volatile init = observation.currentTime.sec() < 25;
//double pWiFiMod = (init) ? (std::pow(pWiFi, 0.1)) : (std::pow(pWiFi, 0.5));
double pWiFiMod = (init) ? (std::pow(pWiFi, 0.5)) : (std::pow(pWiFi, 0.9));
if (quality < 0.2) {
pWiFi = 1;
pWiFiVeto = 1;
std::cout << "disabling WiFi" << std::endl;
}
// WiFi grid
//const MyGridNode& node = grid->getNodeFor(p.state.position);
@@ -379,9 +409,18 @@ public:
//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 pAbsHead = getAbsHead(p.state, observation);
const double prob = pWiFi * pWiFiVeto;// * pAbsHead;
double pGPS = gpsProb.getProbability(p.state.position.inMeter(), observation.gps);
double pAbsHead = getAbsHead(p.state, observation);
double pActivty = getActivity(p.state, observation);
// bad wifi? -> we have no idea where we are!
if (quality < 0.25 && !init) {
pWiFiMod = 1;
//p.weight = std::pow(p.weight, 0.5);
}
// overall evaluation
const double prob = pWiFiMod * pAbsHead * pActivty;
//GPS ERROR?!?!?! does it work without disabling wifi when gps is disabled?
@@ -392,6 +431,11 @@ public:
// TESTING
//p.weight = std::pow(p.weight, 0.5);
// // do NOT update weights
// if (quality < 0.25) {
// p.weight = 1;
// }
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