355 lines
10 KiB
C++
355 lines
10 KiB
C++
#pragma once
|
|
|
|
#include "mesh.h"
|
|
#include "Settings.h"
|
|
#include <omp.h>
|
|
|
|
#include <Indoor/geo/Heading.h>
|
|
#include <Indoor/math/distribution/Uniform.h>
|
|
#include <Indoor/math/distribution/Normal.h>
|
|
//#include <Indoor/math/distribution/Region.h>
|
|
|
|
#include <Indoor/smc/Particle.h>
|
|
#include <Indoor/smc/filtering/ParticleFilter.h>
|
|
#include <Indoor/smc/filtering/ParticleFilterInitializer.h>
|
|
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
|
|
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
|
|
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
|
|
|
|
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
|
|
//#include <Indoor/navMesh/walk/NavMeshWalkEval.h>
|
|
//#include <Indoor/navMesh/walk/NavMeshWalkWifi.h>
|
|
//#include <Indoor/navMesh/walk/NavMeshWalkWifiRegional.h>
|
|
//#include <Indoor/navMesh/walk/NavMeshWalkUnblockable.h>
|
|
//#include <Indoor/navMesh/walk/NavMeshWalkKLD.h>
|
|
#include <Indoor/navMesh/walk/NavMeshWalkSinkOrSwim.h>
|
|
//#include <Indoor/navMesh/NavMeshRandom.h>
|
|
|
|
#include <Indoor/sensors/radio/model/LogDistanceModel.h>
|
|
#include <Indoor/sensors/radio/WiFiMeasurements.h>
|
|
#include <Indoor/data/Timestamp.h>
|
|
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
|
|
#include <Indoor/sensors/activity/ActivityDetector.h>
|
|
|
|
#include "FtmKalman.h"
|
|
|
|
struct MyState {
|
|
|
|
/** the state's position (within the mesh) */
|
|
MyNavMeshLocation pos;
|
|
|
|
/** the state's heading */
|
|
Heading heading;
|
|
|
|
MyState() : pos(), heading(0) {;}
|
|
|
|
MyState(Point3 p) : pos(p, nullptr), heading(0){;}
|
|
|
|
MyState& operator += (const MyState& o) {
|
|
pos.tria = nullptr; // impossible
|
|
pos.pos += o.pos.pos;
|
|
return *this;
|
|
}
|
|
|
|
MyState& operator /= (const double val) {
|
|
pos.tria = nullptr; // impossible
|
|
pos.pos /= val;
|
|
return *this;
|
|
}
|
|
|
|
MyState operator * (const double val) const {
|
|
MyState res;
|
|
res.pos.pos = pos.pos * val;
|
|
return res;
|
|
}
|
|
|
|
float getX(){
|
|
return pos.pos.x;
|
|
}
|
|
|
|
float getY() {
|
|
return pos.pos.y;
|
|
}
|
|
|
|
float getZ() {
|
|
return pos.pos.z;
|
|
}
|
|
|
|
|
|
float getBinValue(const int dim) const {
|
|
switch (dim) {
|
|
case 0: return this->pos.pos.x;
|
|
case 1: return this->pos.pos.y;
|
|
case 2: return this->pos.pos.z;
|
|
case 3: return this->heading.getRAD();
|
|
}
|
|
throw "cant find this value within the bin";
|
|
}
|
|
|
|
};
|
|
|
|
struct MyControl {
|
|
|
|
int numStepsSinceLastEval = 0;
|
|
float headingChangeSinceLastEval = 0;
|
|
|
|
void afterEval() {
|
|
numStepsSinceLastEval = 0;
|
|
headingChangeSinceLastEval = 0;
|
|
}
|
|
|
|
//wifi
|
|
std::map<MACAddress, WiFiMeasurement> wifi;
|
|
|
|
//time
|
|
Timestamp currentTime;
|
|
|
|
//last estimation
|
|
Point3 lastEstimate = Point3(26, 43, 7.5);
|
|
|
|
};
|
|
|
|
struct MyObservation {
|
|
|
|
// pressure
|
|
float sigmaPressure = 0.10f;
|
|
float relativePressure = 0;
|
|
|
|
//wifi
|
|
std::unordered_map<MACAddress, WiFiMeasurement> wifi;
|
|
|
|
//time
|
|
Timestamp currentTime;
|
|
|
|
//activity
|
|
Activity activity;
|
|
|
|
};
|
|
|
|
class MyPFInitUniform : public SMC::ParticleFilterInitializer<MyState> {
|
|
|
|
const MyNavMesh* mesh;
|
|
|
|
public:
|
|
|
|
MyPFInitUniform(const MyNavMesh* mesh) : mesh(mesh) {
|
|
;
|
|
}
|
|
|
|
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
|
|
|
|
/** random position and heading within the mesh */
|
|
Distribution::Uniform<float> dHead(0, 2*M_PI);
|
|
MyNavMeshRandom rnd = mesh->getRandom();
|
|
for (SMC::Particle<MyState>& p : particles) {
|
|
p.state.pos = rnd.draw();
|
|
p.state.heading = dHead.draw();
|
|
p.weight = 1.0 / particles.size();
|
|
}
|
|
}
|
|
|
|
};
|
|
|
|
class MyPFInitFixed : public SMC::ParticleFilterInitializer<MyState> {
|
|
|
|
const MyNavMesh* mesh;
|
|
const Point3 pos;
|
|
|
|
public:
|
|
|
|
MyPFInitFixed(const MyNavMesh* mesh, const Point3 pos) : mesh(mesh), pos(pos) {
|
|
;
|
|
}
|
|
|
|
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
|
|
|
|
/** random position and heading within the mesh */
|
|
Distribution::Uniform<float> dHead(0, 2*M_PI);
|
|
for (SMC::Particle<MyState>& p : particles) {
|
|
p.state.pos = mesh->getLocation(pos);
|
|
p.state.heading = 1.5*M_PI;// dHead.draw();
|
|
p.weight = 1.0 / particles.size();
|
|
}
|
|
}
|
|
|
|
};
|
|
|
|
class MyPFTransStatic : public SMC::ParticleFilterTransition<MyState, MyControl>{
|
|
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
|
|
// nop
|
|
}
|
|
};
|
|
|
|
class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
|
|
|
|
//using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
|
|
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<MyNavMeshTriangle>;
|
|
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<MyNavMeshTriangle>;
|
|
//using MyNavMeshWalk = NM::NavMeshWalkKLD<MyNavMeshTriangle>;
|
|
using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<MyNavMeshTriangle>;
|
|
|
|
MyNavMeshWalk walker;
|
|
|
|
const double lambda = 0.03;
|
|
|
|
public:
|
|
|
|
//std::vector<double> listRadiusSub;
|
|
|
|
MyPFTrans(MyNavMesh& mesh) :
|
|
walker(mesh) {
|
|
|
|
// how to evaluate drawn points
|
|
walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
|
|
walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
|
|
//walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
|
|
}
|
|
|
|
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
|
|
|
|
// walking and heading random
|
|
Distribution::Normal<float> dStepSizeFloor(0.60, 0.1);
|
|
Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
|
|
Distribution::Normal<float> dHeading(0.0, 0.1);
|
|
|
|
#pragma omp parallel for num_threads(3)
|
|
for (int i = 0; i < particles.size(); ++i) {
|
|
SMC::Particle<MyState>& p = particles[i];
|
|
|
|
// how to walk
|
|
MyNavMeshWalkParams params;
|
|
params.heading = p.state.heading + control->headingChangeSinceLastEval + dHeading.draw();
|
|
params.numSteps = control->numStepsSinceLastEval;
|
|
params.start = p.state.pos;
|
|
|
|
params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
|
|
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
|
|
|
|
if(params.stepSizes.stepSizeFloor_m < 0.1 || params.stepSizes.stepSizeStair_m < 0.1){
|
|
params.stepSizes.stepSizeFloor_m = 0.1;
|
|
params.stepSizes.stepSizeStair_m = 0.1;
|
|
}
|
|
|
|
double deltaUnblockable = 0.01;
|
|
|
|
// walk
|
|
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
|
|
//MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi);
|
|
|
|
// assign back to particle's state
|
|
p.weight *= res.probability;
|
|
p.state.pos = res.location;
|
|
p.state.heading = res.heading;
|
|
|
|
}
|
|
|
|
// reset the control (0 steps, 0 delta-heading)
|
|
//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:
|
|
|
|
// 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;
|
|
|
|
//#pragma omp parallel for num_threads(3)
|
|
for (int i = 0; i < particles.size(); ++i) {
|
|
SMC::Particle<MyState>& p = particles[i];
|
|
|
|
double pFtm = 1.0;
|
|
|
|
if (observation.wifi.size() == 0)
|
|
{
|
|
printf("");
|
|
}
|
|
|
|
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)
|
|
)
|
|
{
|
|
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 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 prob = pFtm;
|
|
|
|
if (assignProps)
|
|
p.weight = prob; // p.weight *= prob
|
|
else
|
|
p.weight *= prob;
|
|
|
|
#pragma omp atomic
|
|
sum += prob;
|
|
}
|
|
|
|
return sum;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
using MyFilter = SMC::ParticleFilter<MyState, MyControl, MyObservation>;
|
|
|