FHWS/RussenJournal
Archived
3
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added motion axis stuff

Browse Source
This commit is contained in:
toni
2017-03-02 18:09:09 +01:00
parent 1adb74ec29
commit 09cef9c9aa
5 changed files with 136 additions and 303 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
filter/Logic.h
View File

@@ -9,6 +9,7 @@
#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeading.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingVonMises.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
@@ -30,7 +31,7 @@
#include "../Settings.h"
/** particle-filter init */
/** particle-filter init randomly distributed within the building*/
struct PFInit : public K::ParticleFilterInitializer<MyState> {
Grid<MyNode>& grid;
@@ -51,6 +52,35 @@ struct PFInit : public K::ParticleFilterInitializer<MyState> {
};
/** particle-filter init with fixed position*/
struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
Grid<MyNode>& grid;
GridPoint startPos;
float headingDeg;
PFInitFixed(Grid<MyNode>& grid, GridPoint startPos, float headingDeg) :
grid(grid), startPos(startPos), headingDeg(headingDeg) {;}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
Distribution::Normal<float> norm(0.0f, 1.5f);
for (K::Particle<MyState>& p : particles) {
GridPoint pos = startPos + GridPoint(norm.draw(),norm.draw(),0.0f);
GridPoint startPos = grid.getNodeFor(pos);
p.state.position = startPos; // scatter arround the start position
p.state.heading.direction = headingDeg / 180.0 * M_PI; // fixed heading
p.state.heading.error = 0;
p.state.relativePressure = 0; // start with a relative pressure of 0
}
}
};
/** particle-filter transition */
struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
@@ -60,6 +90,7 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
WalkModuleHeading<MyNode, MyState> modHeadUgly; // stupid
WalkModuleHeadingControl<MyNode, MyState, MyControl> modHead;
WalkModuleHeadingVonMises<MyNode, MyState, MyControl> modHeadMises;
WalkModuleNodeImportance<MyNode, MyState> modImportance;
WalkModuleSpread<MyNode, MyState> modSpread;
WalkModuleFavorZ<MyNode, MyState> modFavorZ;
@@ -68,9 +99,10 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
std::minstd_rand gen;
PFTrans(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) {
PFTrans(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma), modHeadMises(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) {
walker.addModule(&modHead);
//walker.addModule(&modHead);
walker.addModule(&modHeadMises);
//walker.addModule(&modSpread); // might help in some situations! keep in mind!
//walker.addModule(&modHeadUgly);
@@ -136,7 +168,11 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
/** probability for BEACONS */
inline double getBEACON(const MyObs& observation, const GridPoint& point){
return beaconProbability.getProbability(point.inMeter(), observation.currentTime, observation.beacons);
//consider adding the persons height
Point3 p = point.inMeter() + Point3(0,0,1.3);
return beaconProbability.getProbability(p, observation.currentTime, observation.beacons);
}
/** probability for Barometer */
@@ -156,12 +192,13 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
const double pWifi = getWIFI(observation, wifiObs, p.state.position);
const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure);
const double pBeacon = getBEACON(observation, p.state.position);
//small checks
_assertNotNAN(pWifi, "pups");
_assertNotNAN(pWifi, "Wifi prob is nan");
_assertNot0(pBaroPressure,"pBaroPressure is null");
const double prob = pWifi*pBaroPressure;
const double prob = pWifi * pBaroPressure * pBeacon;
p.weight = prob;
sum += (prob);

7
filter/Structs.h
View File

@@ -21,9 +21,11 @@ struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpre
static Floorplan::IndoorMap* map;
float relativePressure = 0;
float relativePressure = 0.0f;
GridPoint positionOld;
MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;}
MyState(GridPoint pos) : WalkState(pos), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;}
@@ -53,6 +55,9 @@ struct MyControl {
/** number of steps since the last transition */
int numStepsSinceLastTransition = 0;
/** current motion delta angle*/
int motionDeltaAngle_rad = 0;
/** reset the control-data after each transition */
void resetAfterTransition() {
turnSinceLastTransition_rad = 0;