141 lines
3.3 KiB
C++
Executable File
141 lines
3.3 KiB
C++
Executable File
#ifndef MYSTATE_H
|
|
#define MYSTATE_H
|
|
|
|
#include <KLib/math/distribution/Normal.h>
|
|
#include <KLib/math/optimization/NumOptVector.h>
|
|
|
|
#include <Indoor/grid/walk/GridWalkState.h>
|
|
|
|
#include "../MyGridNode.h"
|
|
|
|
/**
|
|
* one possible state for the pedestrian
|
|
* 3D position (x, y, floor-nr)
|
|
*/
|
|
struct MyState {
|
|
|
|
// current position
|
|
Point3 pCur;
|
|
|
|
// previous position
|
|
Point3 pOld;
|
|
|
|
// the grid-walk state
|
|
GridWalkState<MyGridNode> walkState;
|
|
|
|
//int distanceWalkedCM;
|
|
|
|
|
|
// double heading_old;
|
|
|
|
// //double transHeading;
|
|
// float numZChanges;
|
|
|
|
// // cumulative distance (in cm) this particle has taken. to-be-reset by the step detector whenever needed!
|
|
// double distanceWalkedCM;
|
|
|
|
double hPa; //relative Pressure given by a history with size defined in BarometerSensorReader.h
|
|
|
|
// double vertical_acc; //vertical acceleration
|
|
|
|
// /** the pedestrian's current heading */
|
|
// double heading;
|
|
|
|
/** empty ctor */
|
|
MyState() : pCur(0,0,0), pOld(0,0,0), walkState(nullptr, Heading(0)) {
|
|
;
|
|
}
|
|
|
|
// /** get the 2D distance between this state and the given x,y (in centimter) */
|
|
// double getDistance2D(const double x_cm, const double y_cm) const {
|
|
// const double dx = (x_cm - this->x_cm);
|
|
// const double dy = (y_cm - this->y_cm);
|
|
// return std::sqrt( (dx*dx) + (dy*dy) );
|
|
// }
|
|
|
|
// /** get the 3D distance between this state and the given x,y,floor (in centimter) */
|
|
// double getDistance3D(const double x_cm, const double y_cm, const double floor_height_cm) const {
|
|
// const double dx = (x_cm - this->x_cm);
|
|
// const double dy = (y_cm - this->y_cm);
|
|
// const double dz = (z_nr - this->z_nr) * floor_height_cm;
|
|
// return std::sqrt( (dx*dx) + (dy*dy) + (dz*dz) );
|
|
// }
|
|
|
|
/** -------- METHODS FOR THE PARTICLE FILTER -------- */
|
|
|
|
MyState& operator += (const MyState& o) {
|
|
pCur += o.pCur;
|
|
hPa += o.hPa;
|
|
//distanceWalked += o.distanceWalked;
|
|
return *this;
|
|
}
|
|
|
|
MyState& operator /= (const double d) {
|
|
pCur /= d;
|
|
hPa /= d;
|
|
//distanceWalked /= d;
|
|
return *this;
|
|
}
|
|
|
|
MyState operator * (const double d) const {
|
|
MyState s = MyState(*this);
|
|
s.pCur *= d;
|
|
s.hPa *= d;
|
|
//distanceWalked *= d;
|
|
return s;
|
|
}
|
|
|
|
|
|
// use the default one
|
|
// MyState& operator = (const MyState& o) {
|
|
// x_cm = o.x_cm;
|
|
// y_cm = o.y_cm;
|
|
// z_nr = o.z_nr;
|
|
// x_cm_old = o.x_cm_old;
|
|
// y_cm_old = o.y_cm_old;
|
|
// z_nr_old = o.z_nr_old;
|
|
// hPa = o.hPa;
|
|
// heading_old = o.heading_old;
|
|
// heading = o.heading;
|
|
// distanceWalkedCM = o.distanceWalkedCM;
|
|
// return *this;
|
|
// }
|
|
|
|
bool belongsToRegion(const MyState& o) const {
|
|
return o.pCur.getDistance(pCur) < 700;
|
|
}
|
|
|
|
// /** rejection for the regional estimator. reject after 150cm distance */
|
|
// bool belongsToRegion(const MyState& o) const {
|
|
|
|
//// // do NOT group particles in distinct floors!
|
|
//// if (z_nr != o.z_nr) {return false;}
|
|
|
|
//// // get the 2D distance
|
|
//// double d = (x_cm - o.x_cm)*(x_cm - o.x_cm) +
|
|
//// (y_cm - o.y_cm)*(y_cm - o.y_cm);
|
|
//// d = std::sqrt(d);
|
|
|
|
//// // 2D distance below grouping threshold?
|
|
//// return d < 350.0;
|
|
|
|
// const double dx = (x_cm - o.x_cm);
|
|
// const double dy = (y_cm - o.y_cm);
|
|
// const double dz = (z_nr - o.z_nr) * 3000;
|
|
|
|
// // get the 2D distance
|
|
// double d = dx*dx + dy*dy + dz*dz;
|
|
// d = std::sqrt(d);
|
|
// return d < 350.0;
|
|
|
|
// }
|
|
|
|
|
|
|
|
// MyState(K::NumOptVector<3>& params) : x_cm(params[0]), y_cm(params[1]), z_cm(params[2]) {;}
|
|
|
|
|
|
};
|
|
|
|
#endif // MYSTATE_H
|