began putting everything together
This commit is contained in:
189
code/particles/MyTransition.h
Executable file
189
code/particles/MyTransition.h
Executable file
@@ -0,0 +1,189 @@
|
||||
#ifndef MYTRANSITION_H
|
||||
#define MYTRANSITION_H
|
||||
|
||||
#include <KLib/math/filter/particles/ParticleFilterTransition.h>
|
||||
#include <KLib/math/distribution/Normal.h>
|
||||
#include <KLib/math/distribution/Uniform.h>
|
||||
|
||||
|
||||
#include <Indoor/grid/Grid.h>
|
||||
#include <Indoor/grid/walk/GridWalk.h>
|
||||
|
||||
#include "MyState.h"
|
||||
#include "MyControl.h"
|
||||
//#include "Helper.h"
|
||||
|
||||
#include "../toni/barometric.h"
|
||||
|
||||
#include "../MyGridNode.h"
|
||||
|
||||
inline double sgn(double x){
|
||||
return ((x>0)?1 : ((x<0)?-1 : 1));
|
||||
}
|
||||
|
||||
class MyTransition : public K::ParticleFilterTransition<MyState, MyControl> {
|
||||
|
||||
private:
|
||||
|
||||
Grid<MyGridNode>& grid;
|
||||
GridWalk<MyGridNode>& walker;
|
||||
|
||||
|
||||
/** a simple normal distribution */
|
||||
K::UniformDistribution distWalkStop;
|
||||
K::NormalDistribution distWalk;
|
||||
K::NormalDistribution distStop;
|
||||
|
||||
|
||||
/** normal distribution for barometer */
|
||||
K::NormalDistribution distBaro;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* ctor
|
||||
* @param choice the choice to use for randomly drawing nodes
|
||||
* @param fp the underlying floorplan
|
||||
*/
|
||||
MyTransition(Grid<MyGridNode>& grid, GridWalk<MyGridNode>& walker) :
|
||||
grid(grid), walker(walker),
|
||||
distWalkStop(0.0, 1.0), distWalk(1.3, 0.5), distStop(0.0, 0.1), distBaro(0.3, 0.05) {
|
||||
|
||||
distWalkStop.setSeed(1234);
|
||||
distWalk.setSeed(1234);
|
||||
distStop.setSeed(1234);
|
||||
|
||||
distBaro.setSeed(5678);
|
||||
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
uint64_t ts = 0;
|
||||
uint64_t deltaMS = 0;
|
||||
|
||||
/** set the current time in millisconds */
|
||||
void setCurrentTime(const uint64_t ts) {
|
||||
if (this->ts == 0) {
|
||||
this->ts = ts;
|
||||
deltaMS = 0;
|
||||
} else {
|
||||
deltaMS = ts - this->ts;
|
||||
this->ts = ts;
|
||||
}
|
||||
}
|
||||
|
||||
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
|
||||
|
||||
for (K::Particle<MyState>& p : particles) {
|
||||
|
||||
// TODO: depending on the time since the last update
|
||||
// random distance to move
|
||||
// const double distance = (distWalkStop.draw() > 0.2) ? (distWalk.draw()) : (distStop.draw());
|
||||
// double dist_m = distance * deltaMS / 1000.0;
|
||||
|
||||
// if (dist_m < 0) {dist_m = -dist_m; p.state.heading = rand() % 360;}
|
||||
|
||||
// update the old heading and the other old values
|
||||
//p.state.walkState.heading = p.state.heading;
|
||||
p.state.pOld = p.state.pCur;
|
||||
|
||||
|
||||
// 10% stand still, 90% walk
|
||||
double dist_m;
|
||||
if (distWalkStop.draw() > 0.9) {
|
||||
dist_m = std::abs(distStop.draw() * deltaMS / 1000.0);
|
||||
} else {
|
||||
dist_m = std::abs(distWalk.draw() * deltaMS / 1000.0);
|
||||
}
|
||||
|
||||
// update cumulative distance
|
||||
p.state.distanceWalkedCM += std::abs(dist_m * 100.0);
|
||||
|
||||
// find the node (square) the particle is within
|
||||
// just to be safe, we round z to the nearest floor
|
||||
//Node3* src = graph->getNearestNode(p.state.x_cm, p.state.y_cm, std::round(p.state.z_nr));
|
||||
const MyGridNode* src = p.state.walkState.node;
|
||||
|
||||
// might happen during initialization:
|
||||
// the particle is nowhere near the grid.. replace it with a random on on the grid
|
||||
// alternative: just ignore.. resampling will fix this issue quickly ;)
|
||||
// if (!src) {
|
||||
// auto it = graph->getNodes().begin();
|
||||
// std::advance(it, rand() % graph->getNodes().size());
|
||||
// src = it->second;
|
||||
// }
|
||||
|
||||
// get new destination
|
||||
//const Node3* dst = choice->getTarget(src, p.state, dist_m);
|
||||
p.state.walkState = walker.getDestination(grid, p.state.walkState, dist_m );
|
||||
|
||||
// randomly move the particle within its target grid (box)
|
||||
// (z remains unchanged!)
|
||||
const int grid_size_cm = grid.getGridSize_cm();
|
||||
|
||||
// new position (x,y) is randomly distributed within the target node
|
||||
Point3 noise = Point3(0,0,0); // TODO
|
||||
p.state.pCur = (Point3) *p.state.walkState.node + noise;
|
||||
|
||||
|
||||
// --- ATTENTION HORRIBLE CODE INCOMING. ---
|
||||
|
||||
// //how many floors are changed? and in what direction (given by the sign)
|
||||
// double numFloorChanged = p.state.z_nr_old - p.state.z_nr;
|
||||
|
||||
// //The height of the single floor levels.
|
||||
// const static double floor_height[3] = {4.1, 3.4, 3.4};
|
||||
|
||||
// //update barometer
|
||||
// if(USE_BAROMETRIC_FORMULAR){
|
||||
// //height the particle has climbed.
|
||||
// double h_1 = 0.0;
|
||||
// double mu = 0.0;
|
||||
|
||||
// //we need only the sign of the floors changed, since the pressure change between the floors
|
||||
// //is calculated within s_getAtmosphericPressure
|
||||
// numFloorChanged = sgn(numFloorChanged);
|
||||
|
||||
// for(int i = std::min(p.state.z_nr_old, p.state.z_nr); i < std::max(p.state.z_nr_old, p.state.z_nr); i++){
|
||||
// h_1 += floor_height[i];
|
||||
// }
|
||||
|
||||
// {
|
||||
// // use the barometric formular to calculate the relative pressure
|
||||
// // the calculation is done assuming sea level height at every floor.
|
||||
// double mslp = BarometricFormular::s_getSeaLevelPressure();
|
||||
// double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0);
|
||||
// mu = std::abs(mslp - pressure);
|
||||
// }
|
||||
|
||||
// if (!USE_STATIC_CIRCULAR_BUFFERING && !USE_DYNAMIC_CIRCULAR_BUFFERING)
|
||||
// p.state.hPa += numFloorChanged * K::NormalDistribution::draw(mu, 0.005);
|
||||
// else
|
||||
// p.state.hPa = numFloorChanged * K::NormalDistribution::draw(mu, 0.15);
|
||||
// }
|
||||
// else{
|
||||
// if (!USE_STATIC_CIRCULAR_BUFFERING && !USE_DYNAMIC_CIRCULAR_BUFFERING)
|
||||
// p.state.hPa += numFloorChanged * distBaro.draw();
|
||||
// else
|
||||
// p.state.hPa = numFloorChanged * distBaro.draw();
|
||||
// }
|
||||
|
||||
// // sanity check
|
||||
// if (p.state.heading != p.state.heading) {throw "detected NaN";}
|
||||
// if (p.state.z_nr != p.state.z_nr) {throw "detected NaN";}
|
||||
// if (p.state.x_cm != p.state.x_cm) {throw "detected NaN";}
|
||||
// if (p.state.y_cm != p.state.y_cm) {throw "detected NaN";}
|
||||
|
||||
// // ensure p.state.z_nr IS discreet
|
||||
// if ( std::abs(p.state.z_nr - std::round(p.state.z_nr)) > 0.01) {throw "detected continuous z_nr!";}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif // MYTRANSITION_H
|
||||
Reference in New Issue
Block a user