diff --git a/competition/src/competition/filter/Logic.h b/competition/src/competition/filter/Logic.h index ad25bef..d214d33 100644 --- a/competition/src/competition/filter/Logic.h +++ b/competition/src/competition/filter/Logic.h @@ -13,6 +13,7 @@ //#include #include #include +#include #include @@ -103,6 +104,7 @@ struct PFTrans : public K::ParticleFilterTransition { // WalkModuleRelativePressureControl modPressure; WalkModuleSpread modSpread; WalkModuleFavorZ modFavorZ; + WalkModuleButterActivity modButterAct; //WalkModuleWiFi modWifi; @@ -113,7 +115,7 @@ struct PFTrans : public K::ParticleFilterTransition { walker.addModule(&modImportance); //walker.addModule(&modPressure); //walker.addModule(&modSpread); - walker.addModule(&modFavorZ); + walker.addModule(&modButterAct); //walker.addModule(&modWifi); } @@ -132,6 +134,9 @@ struct PFTrans : public K::ParticleFilterTransition { const float var = distStep.draw(); const float dist = control->numSteps * var * 0.75; // 75cm + variance for every detected step + //activity + p.state.act = control->barometer.act; + // --------- OLD // transfer from state to walkstate // const MyWalkState start(grid.getNodeFor(p.state.pos), p.state.head, p.state.relPres); @@ -152,8 +157,8 @@ struct PFTrans : public K::ParticleFilterTransition { } - std::cout << "rel pres: " << (tmpSum1/tmpCnt) << std::endl; - std::cout << "rel pres: " << (tmpSum2/tmpCnt) << std::endl; + //std::cout << "rel pres: " << (tmpSum1/tmpCnt) << std::endl; + //std::cout << "rel pres: " << (tmpSum2/tmpCnt) << std::endl; } diff --git a/competition/src/competition/filter/Structs.h b/competition/src/competition/filter/Structs.h index 95e060c..4de4507 100644 --- a/competition/src/competition/filter/Structs.h +++ b/competition/src/competition/filter/Structs.h @@ -11,6 +11,8 @@ #include #include +#include + struct MyState : public WalkState, public WalkStateHeading { static Floorplan::IndoorMap* map; @@ -20,6 +22,7 @@ struct MyState : public WalkState, public WalkStateHeading { // CURRENTLY OBSOLETE (did not work as expected) float relPres; // current pressure relative to t0 + ActivityButterPressure::Activity act; MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0) {;} @@ -98,6 +101,9 @@ struct MyControl { // sensor type 2 float tendence = 0; + // sensor type 3 + ActivityButterPressure::Activity act = ActivityButterPressure::STAY; + } barometer; }; diff --git a/competition/src/competition/main.cpp b/competition/src/competition/main.cpp index 5a8ac48..c5dcf0b 100755 --- a/competition/src/competition/main.cpp +++ b/competition/src/competition/main.cpp @@ -4,6 +4,8 @@ #include "filter/Structs.h" #include "Plotti.h" +#include "Structs.h" + #include "filter/WiFi.h" #include "filter/Logic.h" @@ -73,8 +75,12 @@ void testScaler() { #include -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 mapDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/maps/"; +//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, ...) */ struct DataSetup { @@ -110,7 +116,7 @@ struct Data { DataSetup UAH = { - mapDir + "UAH/UAH7.xml", + mapDir + "UAH/UAH9.xml", { dataDir + "uah/logfile_UAH_R1_S3.txt", @@ -246,14 +252,14 @@ int main(int argc, char** argv) { testScaler(); // the dataset to use - DataSetup setup = data.CAR; + DataSetup setup = data.UAH; // load the floorplan Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); MyState::map = map; // 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 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 @@ -328,6 +334,7 @@ int main(int argc, char** argv) { TurnDetection td; RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); PressureTendence baroTend(Timestamp::fromSec(5)); + ActivityButterPressure act; K::Statistics errorStats; @@ -369,9 +376,12 @@ int main(int argc, char** argv) { relBaro.add(ts, fr.getBarometer()[e.idx].data); //baroTend.add(ts, fr.getBarometer()[e.idx].data); //std::cout << "add tendence" << std::endl; - ctrl.barometer.hPaRelativeToT0 = relBaro.getPressureRealtiveToStart(); - ctrl.barometer.estimatedSigma = relBaro.getSigma(); - ctrl.barometer.tendence = baroTend.get(); + ctrl.barometer.hPaRelativeToT0 = relBaro.getPressureRealtiveToStart(); + ctrl.barometer.estimatedSigma = relBaro.getSigma(); + 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 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 float err_m = mapPos.getDistance(estPos); @@ -429,7 +439,6 @@ int main(int argc, char** argv) { // reset control ctrl.numSteps = 0; ctrl.turnAngle = 0; - }