toni/IPIN2016
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added activity stuff

Browse Source
This commit is contained in:
toni
2016-09-09 17:20:55 +02:00
parent 51d189272d
commit 24604cd467
3 changed files with 34 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
competition/src/competition/filter/Logic.h
View File

@@ -13,6 +13,7 @@
//#include <Indoor/grid/walk/v2/modules/WalkModuleRelativePressureControl.h> //#include <Indoor/grid/walk/v2/modules/WalkModuleRelativePressureControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h> #include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h> #include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleButterActivity.h>
#include <Indoor/grid/factory/v2/GridNodeImportance.h> #include <Indoor/grid/factory/v2/GridNodeImportance.h>
@@ -103,6 +104,7 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
// WalkModuleRelativePressureControl<MyNode, MyState, MyControl> modPressure; // WalkModuleRelativePressureControl<MyNode, MyState, MyControl> modPressure;
WalkModuleSpread<MyNode, MyState> modSpread; WalkModuleSpread<MyNode, MyState> modSpread;
WalkModuleFavorZ<MyNode, MyState> modFavorZ; WalkModuleFavorZ<MyNode, MyState> modFavorZ;
WalkModuleButterActivity<MyNode, MyState> modButterAct;
//WalkModuleWiFi modWifi; //WalkModuleWiFi modWifi;
@@ -113,7 +115,7 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
walker.addModule(&modImportance); walker.addModule(&modImportance);
//walker.addModule(&modPressure); //walker.addModule(&modPressure);
//walker.addModule(&modSpread); //walker.addModule(&modSpread);
walker.addModule(&modFavorZ); walker.addModule(&modButterAct);
//walker.addModule(&modWifi); //walker.addModule(&modWifi);
} }
@@ -132,6 +134,9 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
const float var = distStep.draw(); const float var = distStep.draw();
const float dist = control->numSteps * var * 0.75; // 75cm + variance for every detected step const float dist = control->numSteps * var * 0.75; // 75cm + variance for every detected step
//activity
p.state.act = control->barometer.act;
// --------- OLD // --------- OLD
// transfer from state to walkstate // transfer from state to walkstate
// const MyWalkState start(grid.getNodeFor(p.state.pos), p.state.head, p.state.relPres); // const MyWalkState start(grid.getNodeFor(p.state.pos), p.state.head, p.state.relPres);
@@ -152,8 +157,8 @@ struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
} }
std::cout << "rel pres: " << (tmpSum1/tmpCnt) << std::endl; //std::cout << "rel pres: " << (tmpSum1/tmpCnt) << std::endl;
std::cout << "rel pres: " << (tmpSum2/tmpCnt) << std::endl; //std::cout << "rel pres: " << (tmpSum2/tmpCnt) << std::endl;
} }

6
competition/src/competition/filter/Structs.h
View File

@@ -11,6 +11,8 @@
#include <Indoor/grid/walk/v2/GridWalker.h> #include <Indoor/grid/walk/v2/GridWalker.h>
#include <Indoor/grid/walk/v2/modules/WalkStateHeading.h> #include <Indoor/grid/walk/v2/modules/WalkStateHeading.h>
#include <Indoor/sensors/pressure/ActivityButterPressure.h>
struct MyState : public WalkState, public WalkStateHeading { struct MyState : public WalkState, public WalkStateHeading {
static Floorplan::IndoorMap* map; static Floorplan::IndoorMap* map;
@@ -20,6 +22,7 @@ struct MyState : public WalkState, public WalkStateHeading {
// CURRENTLY OBSOLETE (did not work as expected) // CURRENTLY OBSOLETE (did not work as expected)
float relPres; // current pressure relative to t0 float relPres; // current pressure relative to t0
ActivityButterPressure::Activity act;
MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0) {;} MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0) {;}
@@ -98,6 +101,9 @@ struct MyControl {
// sensor type 2 // sensor type 2
float tendence = 0; float tendence = 0;
// sensor type 3
ActivityButterPressure::Activity act = ActivityButterPressure::STAY;
} barometer; } barometer;
}; };

31
competition/src/competition/main.cpp
View File

@@ -4,6 +4,8 @@
#include "filter/Structs.h" #include "filter/Structs.h"
#include "Plotti.h" #include "Plotti.h"
#include "Structs.h"
#include "filter/WiFi.h" #include "filter/WiFi.h"
#include "filter/Logic.h" #include "filter/Logic.h"
@@ -73,8 +75,12 @@ void testScaler() {
#include <Indoor/geo/Heading.h> #include <Indoor/geo/Heading.h>
const std::string mapDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/maps/"; //const std::string mapDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/maps/";
const std::string dataDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/src/data/"; //const std::string dataDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/src/data/";
//toni
const std::string mapDir = "/home/toni/Documents/programme/localization/IPIN2016/competition/src/competition/map/";
const std::string dataDir = "/home/toni/Documents/programme/localization/IPIN2016/competition/src/competition/data/";
/** describes one dataset (map, training, parameter-estimation, ...) */ /** describes one dataset (map, training, parameter-estimation, ...) */
struct DataSetup { struct DataSetup {
@@ -110,7 +116,7 @@ struct Data {
DataSetup UAH = { DataSetup UAH = {
mapDir + "UAH/UAH7.xml", mapDir + "UAH/UAH9.xml",
{ {
dataDir + "uah/logfile_UAH_R1_S3.txt", dataDir + "uah/logfile_UAH_R1_S3.txt",
@@ -246,14 +252,14 @@ int main(int argc, char** argv) {
testScaler(); testScaler();
// the dataset to use // the dataset to use
DataSetup setup = data.CAR; DataSetup setup = data.UAH;
// load the floorplan // load the floorplan
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
MyState::map = map; MyState::map = map;
// optimize (and save) wifi parameters // optimize (and save) wifi parameters
//optimize(map, setup.scaler, setup.vapMode, setup.minWifiOccurences, setup.training, setup.wifiParams); //optimize(map, setup.scaler, setup.vapMode, setup.minWifiOccurences, setup.training, setup.wifiParams);
// testing // testing
map->floors[0]->obstacles.clear(); map->floors[0]->obstacles.clear();
@@ -273,7 +279,7 @@ int main(int argc, char** argv) {
FileReader fr(setup.training[3]); FileReader fr(setup.training[1]);
//partikel gehen nicht schnell genug nach oben oder unten //partikel gehen nicht schnell genug nach oben oder unten
@@ -328,6 +334,7 @@ int main(int argc, char** argv) {
TurnDetection td; TurnDetection td;
RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
PressureTendence baroTend(Timestamp::fromSec(5)); PressureTendence baroTend(Timestamp::fromSec(5));
ActivityButterPressure act;
K::Statistics<float> errorStats; K::Statistics<float> errorStats;
@@ -369,9 +376,12 @@ int main(int argc, char** argv) {
relBaro.add(ts, fr.getBarometer()[e.idx].data); relBaro.add(ts, fr.getBarometer()[e.idx].data);
//baroTend.add(ts, fr.getBarometer()[e.idx].data); //baroTend.add(ts, fr.getBarometer()[e.idx].data);
//std::cout << "add tendence" << std::endl; //std::cout << "add tendence" << std::endl;
ctrl.barometer.hPaRelativeToT0 = relBaro.getPressureRealtiveToStart(); ctrl.barometer.hPaRelativeToT0 = relBaro.getPressureRealtiveToStart();
ctrl.barometer.estimatedSigma = relBaro.getSigma(); ctrl.barometer.estimatedSigma = relBaro.getSigma();
ctrl.barometer.tendence = baroTend.get(); ctrl.barometer.tendence = baroTend.get();
/** toni activity */
ctrl.barometer.act = act.add(ts, fr.getBarometer()[e.idx].data);
} }
@@ -410,7 +420,7 @@ int main(int argc, char** argv) {
plot.gp << "set label 1001 at screen 0.02, 0.98 'base:" << relBaro.getBaseAvg() << " sigma:" << relBaro.getSigma() << " cur:" << relBaro.getPressureRealtiveToStart() << " hPa " << -relBaro.getPressureRealtiveToStart()/0.10/4.0f << " floor'\n"; plot.gp << "set label 1001 at screen 0.02, 0.98 'base:" << relBaro.getBaseAvg() << " sigma:" << relBaro.getSigma() << " cur:" << relBaro.getPressureRealtiveToStart() << " hPa " << -relBaro.getPressureRealtiveToStart()/0.10/4.0f << " floor'\n";
plot.gp << "set label 1002 at screen 0.02, 0.94 'tend:" << ((std::abs(baroTend.get()) > 0.06) ? "1" : "0") << " val: " << baroTend.get() << "'\n"; plot.gp << "set label 1002 at screen 0.02, 0.94 'tend:" << ((std::abs(baroTend.get()) > 0.06) ? "1" : "0") << " val: " << baroTend.get() << "'\n";
plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" << ctrl.barometer.act << "'\n";
// error between GT and estimation // error between GT and estimation
float err_m = mapPos.getDistance(estPos); float err_m = mapPos.getDistance(estPos);
@@ -429,7 +439,6 @@ int main(int argc, char** argv) {
// reset control // reset control
ctrl.numSteps = 0; ctrl.numSteps = 0;
ctrl.turnAngle = 0; ctrl.turnAngle = 0;
} }