355 lines
10 KiB
C++
355 lines
10 KiB
C++
#ifndef PF_H
|
|
#define PF_H
|
|
|
|
#include <KLib/math/filter/particles/ParticleFilter.h>
|
|
|
|
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
|
|
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
|
|
|
|
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
|
|
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
|
|
|
|
#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/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>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
|
|
#include <Indoor/grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h>
|
|
|
|
#include "../Settings.h"
|
|
|
|
struct MyGridNode : public GridNode, public GridPoint {//, public WiFiGridNode<30> {
|
|
|
|
float navImportance;
|
|
float getNavImportance() const { return navImportance; }
|
|
|
|
float walkImportance;
|
|
float getWalkImportance() const { return walkImportance; }
|
|
|
|
|
|
/** empty ctor */
|
|
MyGridNode() : GridPoint(-1, -1, -1) {;}
|
|
|
|
/** ctor */
|
|
MyGridNode(const int x_cm, const int y_cm, const int z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
|
|
|
|
|
|
static void staticDeserialize(std::istream& inp) {
|
|
//WiFiGridNode::staticDeserialize(inp);
|
|
}
|
|
|
|
static void staticSerialize(std::ostream& out) {
|
|
//WiFiGridNode::staticSerialize(out);
|
|
}
|
|
|
|
};
|
|
|
|
struct MyState : public WalkState, public WalkStateFavorZ, public WalkStateHeading {
|
|
|
|
|
|
/** ctor */
|
|
MyState(const int x_cm, const int y_cm, const int z_cm) : WalkState(GridPoint(x_cm, y_cm, z_cm)), WalkStateHeading(Heading(0), 0) {
|
|
;
|
|
}
|
|
|
|
MyState() : WalkState(GridPoint()), WalkStateHeading(Heading(0), 0) {
|
|
;
|
|
}
|
|
|
|
MyState& operator += (const MyState& o) {
|
|
position += o.position;
|
|
return *this;
|
|
}
|
|
|
|
MyState& operator /= (const float val) {
|
|
position /= val;
|
|
return *this;
|
|
}
|
|
|
|
MyState operator * (const float val) const {
|
|
MyState copy = *this;
|
|
copy.position = copy.position * val;
|
|
return copy;
|
|
}
|
|
|
|
};
|
|
|
|
/** observed sensor data */
|
|
struct MyObservation {
|
|
|
|
/** wifi measurements */
|
|
WiFiMeasurements wifi;
|
|
|
|
/** gps measurements */
|
|
GPSData gps;
|
|
|
|
// TODO: switch to a general activity enum/detector for barometer + accelerometer + ...?
|
|
/** detected activity */
|
|
ActivityButterPressure::Activity activity;
|
|
|
|
/** time of evaluation */
|
|
Timestamp currentTime;
|
|
|
|
};
|
|
|
|
/** (observed) control data */
|
|
struct MyControl {
|
|
|
|
/** turn angle (in radians) since the last transition */
|
|
float turnSinceLastTransition_rad = 0;
|
|
|
|
/** number of steps since the last transition */
|
|
int numStepsSinceLastTransition = 0;
|
|
|
|
/** absolute heading in radians */
|
|
float compassAzimuth_rad = 0;
|
|
|
|
// TODO: switch to a general activity enum/detector using barometer + accelerometer?
|
|
/** currently detected activity */
|
|
ActivityButterPressure::Activity activity = ActivityButterPressure::Activity::STAY;
|
|
|
|
Activity activityNew;
|
|
|
|
/** reset the control-data after each transition */
|
|
void resetAfterTransition() {
|
|
turnSinceLastTransition_rad = 0;
|
|
numStepsSinceLastTransition = 0;
|
|
}
|
|
|
|
};
|
|
|
|
class PFInit : public K::ParticleFilterInitializer<MyState> {
|
|
|
|
private:
|
|
|
|
Grid<MyGridNode>* grid;
|
|
|
|
public:
|
|
|
|
PFInit(Grid<MyGridNode>* grid) : grid(grid) {
|
|
|
|
}
|
|
|
|
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
|
|
|
|
std::minstd_rand gen;
|
|
std::uniform_int_distribution<int> distIdx(0, grid->getNumNodes()-1);
|
|
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];
|
|
p.state.position = node; // random position
|
|
p.state.heading.direction = Heading(distHead(gen)); // random heading
|
|
p.weight = 1.0 / particles.size(); // equal weight
|
|
}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
class PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
|
|
|
|
public:
|
|
|
|
/** local, static control-data COPY */
|
|
MyControl ctrl;
|
|
|
|
Grid<MyGridNode>* grid;
|
|
GridWalker<MyGridNode, MyState> walker;
|
|
|
|
WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
|
|
WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
|
|
WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
|
|
WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
|
|
|
|
WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modRelHead;
|
|
WalkModuleAbsoluteHeadingControl<MyGridNode, MyState, MyControl> modAbsHead;
|
|
|
|
std::minstd_rand gen;
|
|
|
|
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(&modActivity);
|
|
|
|
//walker.addModule(&modFavorZ);
|
|
//walker.addModule(&modImportance);
|
|
|
|
// if (Settings::destination != GridPoint(0,0,0)) {
|
|
// //walker.addModule(&modDestination);
|
|
// modDestination.setDestination(grid->getNodeFor(Settings::destination));
|
|
// }
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* _ctrl) override {
|
|
|
|
// 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);
|
|
|
|
// 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));
|
|
|
|
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);
|
|
//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.1); // add 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;}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
|
|
|
|
Grid<MyGridNode>* grid;
|
|
|
|
WiFiModelLogDistCeiling& wifiModel;
|
|
|
|
EarthMapping& em;
|
|
|
|
|
|
WiFiObserverFree wiFiProbability; // free-calculation
|
|
//WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation
|
|
|
|
// smartphone is 1.3 meter above ground
|
|
const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);
|
|
|
|
public:
|
|
|
|
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
|
|
|
|
|
|
}
|
|
|
|
double getStairProb(const K::Particle<MyState>& p, const ActivityButterPressure::Activity act) {
|
|
|
|
const float kappa = 0.75;
|
|
|
|
const MyGridNode& gn = grid->getNodeFor(p.state.position);
|
|
switch (act) {
|
|
|
|
case ActivityButterPressure::Activity::STAY:
|
|
if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;}
|
|
if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;}
|
|
{return 1-kappa;}
|
|
|
|
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;}
|
|
|
|
}
|
|
|
|
return 1.0;
|
|
|
|
}
|
|
|
|
|
|
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);
|
|
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
|
|
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
|
|
|
|
//#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);
|
|
|
|
// WiFi grid
|
|
//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;
|
|
|
|
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
|
|
if (p.weight != p.weight) {throw Exception("nan");}
|
|
|
|
#pragma omp atomic
|
|
sum += p.weight;
|
|
|
|
}
|
|
|
|
return sum;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // PF_H
|