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

Particle reduced to ftm eval only

Browse Source
This commit is contained in:
Markus Bullmann
2019-09-25 09:30:16 +02:00
parent 36c64b5032
commit 24bbc56f28
6 changed files with 908 additions and 285 deletions
Download Patch File Download Diff File Expand all files Collapse all files

84
code/filter.h
View File

@@ -3,6 +3,7 @@
#include "mesh.h"
#include "Settings.h"
#include <omp.h>
#include <array>
#include <Indoor/geo/Heading.h>
#include <Indoor/math/distribution/Uniform.h>
@@ -111,19 +112,13 @@ struct MyControl {
struct MyObservation {
// pressure
float sigmaPressure = 0.10f;
float relativePressure = 0;
//wifi
std::unordered_map<MACAddress, WiFiMeasurement> wifi;
std::unordered_map<MACAddress, WiFiMeasurement> wifi; // deprecated
std::array<float, 4> dists;
std::array<float, 4> sigmas; // from kalman
//time
Timestamp currentTime;
//activity
Activity activity;
};
class MyPFInitUniform : public SMC::ParticleFilterInitializer<MyState> {
@@ -260,44 +255,15 @@ public:
//control->afterEval();
}
};
class MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
//TODO: add this to transition probability
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
const float kappa = 0.75;
switch (act) {
case Activity::WALKING:
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
{return 1-kappa;}
case Activity::WALKING_UP:
case Activity::WALKING_DOWN:
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;}
{return 1-kappa;}
}
return 1.0;
}
public:
struct MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
// FRANK
MyPFEval() { };
bool assignProps = false;
std::shared_ptr<std::unordered_map<MACAddress, Kalman>> kalmanMap;
virtual double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& observation) override {
double sum = 0;
@@ -308,47 +274,27 @@ public:
double pFtm = 1.0;
if (observation.wifi.size() == 0)
for (size_t i = 0; i < 4; i++)
{
printf("");
}
float dist = observation.dists[i];
const float sigma = isnan(observation.sigmas[i]) ? 3.5 : observation.sigmas[i];
for (auto& wifi : observation.wifi) {
if ( (true && wifi.second.getAP().getMAC() == Settings::NUC1)
|| (true && wifi.second.getAP().getMAC() == Settings::NUC2)
|| (true && wifi.second.getAP().getMAC() == Settings::NUC3)
|| (true && wifi.second.getAP().getMAC() == Settings::NUC4)
)
if (!isnan(dist))
{
float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(wifi.second.getAP().getMAC()).rssi_pathloss;
float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.second.getRSSI());
float ftmDist = wifi.second.getFtmDist();
Point3 apPos = Settings::data.CurrentPath.NUCs.find(wifi.first)->second.position;
Point3 apPos = Settings::data.CurrentPath.nucInfo(i).position;
Point3 particlePos = p.state.pos.pos;
particlePos.z = 1.3; // smartphone höhe
float apDist = particlePos.getDistance(apPos);
if (Settings::UseKalman)
{
auto kalman = kalmanMap->at(wifi.second.getAP().getMAC());
pFtm *= Distribution::Normal<float>::getProbability(ftmDist, std::sqrt(kalman.P(0,0)), apDist);
}
else
{
pFtm *= Distribution::Normal<float>::getProbability(apDist, 3.5, ftmDist);
//pFtm *= Distribution::Region<float>::getProbability(apDist, 3.5/2, ftmDist);
}
}
double x = Distribution::Normal<double>::getProbability(dist, std::sqrt(sigma), apDist);
pFtm *= x;
}
}
double prob = pFtm;
if (assignProps)
p.weight = prob; // p.weight *= prob
p.weight = prob;
else
p.weight *= prob;
@@ -357,9 +303,7 @@ public:
}
return sum;
}
};