FrankE
deb21fc550
added new eval using shortest-path + plotting removed compiler warnings for clean-code fixed some minor issues added new TeX code and new graphics
126 lines
3.0 KiB
C++
Executable File
126 lines
3.0 KiB
C++
Executable File
#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 distWalkPerSec;
|
|
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), distWalkPerSec(1.0, 0.3), distStop(0.0, 0.1), distBaro(0.3, 0.05) {
|
|
|
|
distWalkStop.setSeed(1234);
|
|
distWalkPerSec.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(distWalkPerSec.draw() * deltaMS / 1000.0);
|
|
// }
|
|
|
|
// get new destination
|
|
//const Node3* dst = choice->getTarget(src, p.state, dist_m);
|
|
p.state.walkState = walker.getDestination(grid, p.state.walkState, control->walked_m, control->headingChange_rad );
|
|
|
|
// 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;
|
|
|
|
|
|
// update the baromter
|
|
p.state.hPa += (p.state.pOld.z - p.state.pCur.z) / 100.0f * 0.105f;
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // MYTRANSITION_H
|