diff --git a/competition/src/competition/filter/Logic.h b/competition/src/competition/filter/Logic.h index d214d33..d677f29 100644 --- a/competition/src/competition/filter/Logic.h +++ b/competition/src/competition/filter/Logic.h @@ -10,10 +10,10 @@ #include #include #include -//#include #include #include #include +#include #include @@ -101,27 +101,29 @@ struct PFTrans : public K::ParticleFilterTransition { WalkModuleHeading modHeadUgly; // stupid WalkModuleHeadingControl modHead; WalkModuleNodeImportance modImportance; -// WalkModuleRelativePressureControl modPressure; WalkModuleSpread modSpread; WalkModuleFavorZ modFavorZ; WalkModuleButterActivity modButterAct; - - //WalkModuleWiFi modWifi; + //WalkModulePreventVisited modPreventVisited; - PFTrans(Grid& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, 3.5f) {//, modPressure(ctrl, 0.100) { + + PFTrans(Grid& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, 4.5f) {//, modPressure(ctrl, 0.100) { + walker.addModule(&modHead); + //walker.addModule(&modSpread); // might help in some situations! keep in mind! + //walker.addModule(&modHeadUgly); walker.addModule(&modImportance); - //walker.addModule(&modPressure); - //walker.addModule(&modSpread); - walker.addModule(&modButterAct); + walker.addModule(&modFavorZ); + walker.addModule(&modButterAct); //walker.addModule(&modWifi); + //walker.addModule(&modPreventVisited); } virtual void transition(std::vector>& particles, const MyControl* control) override { - Distribution::Normal distStep(1, 0.10); + Distribution::Normal distStep(1, 0.15); float tmpSum1 = 0; float tmpSum2 = 0; @@ -132,7 +134,7 @@ struct PFTrans : public K::ParticleFilterTransition { tmpSum1 += p.state.relPres; 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.76; // 75cm + variance for every detected step //activity p.state.act = control->barometer.act; diff --git a/competition/src/competition/filter/Structs.h b/competition/src/competition/filter/Structs.h index 4de4507..5550eae 100644 --- a/competition/src/competition/filter/Structs.h +++ b/competition/src/competition/filter/Structs.h @@ -9,11 +9,14 @@ #include #include -#include +#include +#include +#include +#include #include -struct MyState : public WalkState, public WalkStateHeading { +struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpread, public WalkStateFavorZ { static Floorplan::IndoorMap* map; diff --git a/competition/src/competition/filter/WiFi.h b/competition/src/competition/filter/WiFi.h index 4c130df..d285fb5 100644 --- a/competition/src/competition/filter/WiFi.h +++ b/competition/src/competition/filter/WiFi.h @@ -132,11 +132,11 @@ public: // high signal strength? higher sigma! const Timestamp age = curTS - ap.getTimestamp(); - //const float sigma = 10.0 + 0.5 * age.sec() + rssiSigma(scanRSSI); - //double apProb = Distribution::Normal::getProbability(modelRSSI, sigma, scanRSSI); + const float sigma = 11.0 + 1.2 * age.sec() + rssiSigma(scanRSSI); + double apProb = Distribution::Normal::getProbability(modelRSSI, sigma, scanRSSI); - const float sigma = 8.0 + 0.5 * age.sec() + rssiSigma(scanRSSI); - const double apProb = Distribution::Region::getProbability(modelRSSI, sigma, scanRSSI); + //const float sigma = 8.0 + 0.5 * age.sec() + rssiSigma(scanRSSI); + //const double apProb = Distribution::Region::getProbability(modelRSSI, sigma, scanRSSI); prob += std::log(apProb); diff --git a/competition/src/competition/main.cpp b/competition/src/competition/main.cpp index c5dcf0b..1e7a370 100755 --- a/competition/src/competition/main.cpp +++ b/competition/src/competition/main.cpp @@ -75,12 +75,13 @@ 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/"; +//frank +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/"; +//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 { @@ -101,7 +102,7 @@ struct Data { { dataDir + "car/logfile_CAR_R1_S3.txt", - dataDir + "car/logfile_CAR_R1_S3mini.txt", + dataDir + "car/logfile_CAR_R1_S3mini.txt", // s3 mini suckt hier genauso wie bei uns dataDir + "car/logfile_CAR_R2_S3.txt", dataDir + "car/logfile_CAR_R2_S4.txt" }, @@ -252,14 +253,14 @@ int main(int argc, char** argv) { testScaler(); // the dataset to use - DataSetup setup = data.UAH; + 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(); @@ -279,7 +280,7 @@ int main(int argc, char** argv) { - FileReader fr(setup.training[1]); + FileReader fr(setup.training[1]); //partikel gehen nicht schnell genug nach oben oder unten @@ -305,7 +306,7 @@ int main(int argc, char** argv) { MyControl ctrl; ctrl.numSteps = 0; MyObs obs; - int numParticles = 10000; + int numParticles = 4000; PFEval* eval = new PFEval(setup.scaler, model); @@ -320,7 +321,7 @@ int main(int argc, char** argv) { //pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); //pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationRegionalWeightedAverage())); - pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationOrderedWeightedAverage(0.6))); + pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationOrderedWeightedAverage(0.95))); //pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationKernelDensity()));