diff --git a/code/CMakeLists.txt.user b/code/CMakeLists.txt.user index 1e274f6..86681f7 100644 --- a/code/CMakeLists.txt.user +++ b/code/CMakeLists.txt.user @@ -1,6 +1,6 @@ - + EnvironmentId @@ -40,6 +40,7 @@ true false 0 + true true 0 8 @@ -65,15 +66,17 @@ 0 0 - - false + + CMAKE_CXX_COMPILER:FILEPATH=/usr/bin/c++ + QT_QMAKE_EXECUTABLE:STRING=%{Qt:qmakeExecutable} + /home/toni/Documents/programme/localization/IPIN2016/build-code-Desktop-Default - - false - + + all + true Make @@ -86,10 +89,10 @@ - clean - - true - + + + clean + true Make @@ -108,15 +111,16 @@ CMakeProjectManager.CMakeBuildConfiguration - -DCMAKE_BUILD_TYPE=Debug - false + + CMAKE_BUILD_TYPE:STRING=Debug + /home/toni/Documents/programme/localization/IPIN2016/build-code-Desktop-Debug - - false - + + + true Make @@ -129,10 +133,10 @@ - clean - - true - + + + clean + true Make @@ -151,15 +155,16 @@ CMakeProjectManager.CMakeBuildConfiguration - -DCMAKE_BUILD_TYPE=Release - false + + CMAKE_BUILD_TYPE:STRING=Release + /home/toni/Documents/programme/localization/IPIN2016/build-code-Desktop-Release - - false - + + + true Make @@ -172,10 +177,10 @@ - clean - - true - + + + clean + true Make @@ -194,15 +199,16 @@ CMakeProjectManager.CMakeBuildConfiguration - -DCMAKE_BUILD_TYPE=RelWithDebInfo - false + + CMAKE_BUILD_TYPE:STRING=RelWithDebInfo + /home/toni/Documents/programme/localization/IPIN2016/build-code-Desktop-Release with Debug Information - - false - + + + true Make @@ -215,10 +221,10 @@ - clean - - true - + + + clean + true Make @@ -237,15 +243,16 @@ CMakeProjectManager.CMakeBuildConfiguration - -DCMAKE_BUILD_TYPE=MinSizeRel - false + + CMAKE_BUILD_TYPE:STRING=MinSizeRel + /home/toni/Documents/programme/localization/IPIN2016/build-code-Desktop-Minimum Size Release - - false - + + + true Make @@ -258,10 +265,10 @@ - clean - - true - + + + clean + true Make @@ -295,6 +302,7 @@ 1 + false false 1000 @@ -336,6 +344,7 @@ IPIN2016 + /home/toni/Documents/programme/localization/IPIN2016/build-code-Desktop-Default 2 IPIN2016 diff --git a/competition/src/competition/CMakeLists.txt b/competition/src/competition/CMakeLists.txt index e315d2e..da64fea 100755 --- a/competition/src/competition/CMakeLists.txt +++ b/competition/src/competition/CMakeLists.txt @@ -19,6 +19,9 @@ ENDIF() INCLUDE_DIRECTORIES( ../ + ../../ + ../../../ + ../../../../ ) @@ -38,7 +41,7 @@ FILE(GLOB SOURCES ./*/*.cpp ./*/*/*.cpp ./*/*/*/*.cpp - ../Indoor/lib/tinyxml/tinyxml2.cpp + ../../../../Indoor/lib/tinyxml/tinyxml2.cpp ) @@ -55,7 +58,7 @@ ADD_DEFINITIONS( -fstack-protector-all -g3 - -O2 + -O2 -march=native -DWITH_TESTS @@ -65,6 +68,13 @@ ADD_DEFINITIONS( ) +# allow OMP +find_package(OpenMP) +if (OPENMP_FOUND) + set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +endif() + # build a binary file ADD_EXECUTABLE( diff --git a/competition/src/competition/FileReader.h b/competition/src/competition/FileReader.h index 826fd30..6164b81 100644 --- a/competition/src/competition/FileReader.h +++ b/competition/src/competition/FileReader.h @@ -164,7 +164,7 @@ private: int i = 0; (void) i; - if (what == "WIFI") {parseWiFi(appTS, sensTS, data);} + if (what == "WIFI") {parseWiFi(appTS, sensTS, data);} else if (what == "POSI") {parsePosition(appTS, sensTS, data);} else if (what == "ACCE") {parseAccelerometer(appTS, sensTS, data);} else if (what == "GYRO") {parseGyroscope(appTS, sensTS, data);} @@ -257,12 +257,12 @@ private: // create a new empty scan-entry wifi.push_back(TS(appTS, appTS, WiFiMeasurements())); - entries.push_back(Entry(Sensor::WIFI, appTS, wifi.size()-1)); + entries.push_back(Entry(Sensor::WIFI, appTS, wifi.size()-1)); } // append AP to current scan-entry - wifi.back().data.entries.push_back(WiFiMeasurement(AccessPoint(mac, ssid), std::stoi(rssi), Timestamp::fromSec(appTS))); + wifi.back().data.entries.push_back(WiFiMeasurement(AccessPoint(mac, ssid), std::stoi(rssi), Timestamp::fromSec(appTS))); } diff --git a/competition/src/competition/Plotti.h b/competition/src/competition/Plotti.h index aa81c33..4dc50fb 100644 --- a/competition/src/competition/Plotti.h +++ b/competition/src/competition/Plotti.h @@ -19,6 +19,8 @@ #include +#include "filter/WiFi.h" + struct Plotti { K::Gnuplot gp; @@ -32,6 +34,8 @@ struct Plotti { K::GnuplotSplotElementPoints pParticles; K::GnuplotSplotElementColorPoints pColorPoints; K::GnuplotSplotElementLines gtPath; + K::GnuplotSplotElementLines estPath; + K::GnuplotSplotElementLines estPathSmoothed; Plotti() { gp << "set xrange[0-50:70+50]\nset yrange[0-50:50+50]\nset ticslevel 0\n"; @@ -44,7 +48,8 @@ struct Plotti { splot.add(&pStairs); pStairs.setColorHex("#000000"); splot.add(&pInterest); pInterest.setPointSize(2); pInterest.setColorHex("#ff0000"); splot.add(>Path); gtPath.setLineWidth(2); gtPath.setColorHex("#000000"); - + splot.add(&estPath); estPath.setLineWidth(2); estPath.setColorHex("#00ff00"); + splot.add(&estPathSmoothed); estPathSmoothed.setLineWidth(2); estPathSmoothed.setColorHex("#0000ff"); } void addLabel(const int idx, const Point3 p, const std::string& str, const int fontSize = 10) { @@ -80,21 +85,35 @@ struct Plotti { gtPath.add(gp); } + // estimated path + void addEstimationNode(const Point3 pos){ + K::GnuplotPoint3 est(pos.x, pos.y, pos.z); + estPath.add(est); + } -// void debugWiFi(const WiFiModelX& model, const APScan& scan, const float curTS) { + // estimated path + void addEstimationNodeSmoothed(const Point3 pos){ + K::GnuplotPoint3 est(pos.x, pos.y, pos.z); + estPathSmoothed.add(est); + } -// pColorPoints.clear(); -// const float step = 2.0; -// float z = 0; -// for (float x = -20; x < 90; x += step) { -// for (float y = -10; y < 60; y += step) { -// double prob = model.getProbability(scan, Point3(x,y,z), curTS); -// pColorPoints.add(K::GnuplotPoint3(x,y,z), prob); -// } -// } -// } + void debugWiFi(const WiFiModelX& model, const WiFiMeasurements& scan, const float curTS) { + + pColorPoints.clear(); + + const float step = 2.0; + float z = 0; + for (float x = -20; x < 200; x += step) { + for (float y = -10; y < 200; y += step) { + const Point3 pt(x,y,z); + double prob = model.getProbability(scan, pt, Timestamp::fromSec(curTS)); + pColorPoints.add(K::GnuplotPoint3(x,y,z), prob); + } + } + + } template void debugProb(Grid& grid, std::function func, const MyObs& obs) { @@ -205,9 +224,42 @@ struct Plotti { void show() { gp.draw(splot); - gp.flush(); + gp.flush(); } + void saveToFile(std::ofstream& stream){ + gp.draw(splot); + stream << "set terminal x11 size 2000,1500\n"; + stream << gp.getBuffer(); + stream << "pause -1\n"; + gp.flush(); + } + + void printSingleFloor(const std::string& path, const int floorNum) { + gp << "set terminal png size 1280,720\n"; + gp << "set output '" << path << "_" << floorNum <<".png'\n"; + gp << "set view 0,0\n"; + gp << "set zrange [" << (floorNum * 4) - 2 << " : " << (floorNum * 4) + 2 << "]\n"; + gp << "set autoscale xy\n"; + } + + void printSideView(const std::string& path, const int degree) { + gp << "set terminal png size 1280,720\n"; + gp << "set output '" << path << "_deg" << degree <<".png'\n"; + gp << "set view 90,"<< degree << "\n"; + gp << "set autoscale xy\n"; + gp << "set autoscale z\n"; + } + + void printOverview(const std::string& path) { + gp << "set terminal png size 1280,720\n"; + gp << "set output '" << path << "_overview" << ".png'\n"; + gp << "set view 75,60\n"; + gp << "set autoscale xy\n"; + gp << "set autoscale z\n"; + } + + }; #endif // PLOTTI_H diff --git a/competition/src/competition/Scaler.h b/competition/src/competition/Scaler.h index f9bc0ec..1f31c4a 100644 --- a/competition/src/competition/Scaler.h +++ b/competition/src/competition/Scaler.h @@ -7,7 +7,7 @@ struct IPIN { double lat; double lon; - double floorNr; + double floorNr; IPIN(const double lat, const double lon, const double floorNr) : lat(lat), lon(lon), floorNr(floorNr) {;} }; diff --git a/competition/src/competition/Structs.h b/competition/src/competition/Structs.h index 296bcf5..e2d7361 100644 --- a/competition/src/competition/Structs.h +++ b/competition/src/competition/Structs.h @@ -82,6 +82,4 @@ struct GPS { - - #endif // STRUCTS_H diff --git a/competition/src/competition/data/car/wifiParams.txt b/competition/src/competition/data/car/wifiParams.txt new file mode 100644 index 0000000..f684cd4 --- /dev/null +++ b/competition/src/competition/data/car/wifiParams.txt @@ -0,0 +1,20 @@ +E0:91:53:1B:36:F0,25.9334,38.9436,-0.0524627,-39.1386,3.49982,-9.36092 +00:0B:86:27:35:80,10.4727,31.8167,-2.71109,-35.0022,3.44592,-10.3489 +00:0B:86:27:35:90,90.3394,15.4303,2.04479,-49.8716,2.68872,-6.04702 +00:0B:86:27:3E:E0,6.17818,4.15351,0.139055,-35,3.4997,-5.7909 +00:90:4C:91:00:00,83.9248,11.7224,12.29,-49.9992,2.82737,-7.04282 +B8:A3:86:65:E3:20,4.69674,5.5026,0.464426,-40.543,3.49904,-7.30668 +1A:FE:34:9D:0D:20,31.2992,33.9625,0.145345,-41.5356,3.49953,-4.3978 +72:77:81:A4:9E:30,28.7311,38.3731,6.2193,-49.9932,3.21405,-5.31385 +A0:2B:B8:6E:F8:00,-5.31735,104.568,4.70765,-49.9934,2.06603,-9.35664 +00:80:2F:17:A2:B0,25.0241,36.8684,-0.150894,-45.8281,3.43221,-7.33956 +00:0B:86:27:3E:D0,36.061,33.8152,0.0645585,-40.771,3.49971,-6.54765 +00:80:2F:19:26:70,27.4261,37.3301,0.866323,-49.9991,3.08838,-5.47872 +82:B6:BE:58:5C:50,62.9964,33.7411,0.140919,-49.9989,2.72436,-9.15091 +00:0B:86:27:36:C0,53.7462,27.6526,-0.0873488,-36.7065,3.27317,-5.56178 +00:0B:86:27:32:E0,63.4623,32.099,-0.0429999,-49.9917,2.4404,-9.97059 +00:0B:86:27:35:10,34.2176,6.17748,-0.0693621,-41.6553,3.49999,-6.08537 +00:0B:86:27:3E:80,18.4034,22.1678,1.00951,-38.4197,3.49124,-9.24767 +00:0B:86:27:37:B0,30.3648,26.754,1.16354,-42.6981,3.4995,-5.50056 +00:0B:86:27:45:B0,57.2717,7.33425,-0.164426,-39.1558,3.49748,-7.04232 +F8:8E:85:CF:2D:D0,39.8152,108.911,11.3717,-49.9954,2.12391,-7.82796 diff --git a/competition/src/competition/data/uah/wifiParams.txt b/competition/src/competition/data/uah/wifiParams.txt new file mode 100644 index 0000000..e23daa6 --- /dev/null +++ b/competition/src/competition/data/uah/wifiParams.txt @@ -0,0 +1,133 @@ +04:BD:88:0D:9B:A0,214.378,59.8349,-4.39663,-46.6016,2.10851,-1.99032 +1C:65:9D:6A:7D:30,122.755,75.7127,-0.110118,-49.9998,2.91062,-0.386257 +04:BD:88:0D:D0:80,118.211,45.4233,10.423,-35.0017,3.43328,-6.92199 +62:6D:C7:27:B7:60,119.496,64.0401,-2.88946,-42.1221,2.85472,-3.99463 +C8:BE:19:62:A3:E0,96.1187,84.3896,3.9973,-35.0055,2.98134,-2.70863 +00:12:17:49:7E:90,93.3295,93.3709,11.9974,-35.0002,2.48209,-7.32168 +24:DE:C6:61:B2:70,52.3967,77.2715,3.90902,-35.0016,3.08409,-1.19813 +04:BD:88:50:43:70,130.005,54.107,-0.00540396,-49.9837,1.8371,-6.9551 +00:12:17:49:7E:A0,123.406,62.3925,14.0466,-45.6587,3.21297,-0.00168633 +00:18:E7:DA:7F:20,125.466,46.1327,10.4264,-49.3228,2.68111,-1.92762 +04:BD:88:50:4A:30,129.904,73.8747,-3.41909,-49.9796,2.25125,-3.09197 +04:BD:88:0D:9F:00,130.35,57.4257,4.55298,-35.003,3.36798,-5.53225 +04:BD:88:50:38:20,127.285,104.478,-0.380571,-41.2844,2.33703,-3.1488 +04:BD:88:0D:9F:10,128.104,56.6187,5.50136,-37.9741,3.24012,-6.54794 +04:BD:88:50:48:80,69.6526,3.04814,-0.000448067,-40.3957,2.59139,-4.31169 +04:BD:88:0D:72:C0,127.869,78.8776,-0.255026,-49.9348,2.68001,-0.756667 +00:1C:F0:62:62:D0,134.013,66.6918,8.91396,-35.7719,3.49942,-1.36856 +04:BD:88:50:4D:E0,62.0733,9.79976,-2.95398,-49.9807,2.37476,-5.22889 +00:13:EF:74:06:20,56.9621,6.33708,31.1861,-42.6515,2.79413,-2.35615 +04:BD:88:50:3D:C0,141.226,56.7454,1.2621,-35.002,2.99213,-5.93712 +04:BD:88:50:4A:20,128.428,74.9341,1.16055,-42.8032,2.8994,-2.80893 +04:BD:88:50:4B:60,157.182,71.1934,7.01438,-48.4553,1.865,-11.0567 +04:BD:88:0D:B3:40,35.6647,-25.3662,3.98375,-49.9831,1.91541,-0.877453 +04:BD:88:50:3D:D0,145.06,49.5471,1.07102,-35.0031,3.0624,-7.21522 +04:BD:88:50:43:60,131.5,51.6265,-4.86944,-37.957,2.95518,-3.53852 +00:1F:F3:C4:C8:C0,73.6514,16.9333,10.6067,-42.6721,3.49909,-1.81325 +04:BD:88:50:4B:70,149.253,69.6315,12.0018,-46.693,1.4728,-6.50942 +04:BD:88:0D:B3:50,85.0855,32.5164,3.97938,-49.9982,2.29552,-2.71004 +04:BD:88:50:3D:00,185.468,70.2744,14.4805,-40.5078,1.03565,-9.87018 +04:BD:88:0D:6E:E0,121.157,61.9996,13.7546,-40.2715,3.41487,-0.980417 +04:BD:88:50:4E:60,117.834,52.7936,-0.0048793,-35.1553,2.93496,-5.92663 +04:BD:88:0D:99:00,123.013,82.4264,7.42219,-49.9983,2.67541,-0.308817 +24:DE:C6:61:B6:80,70.7065,74.5002,-2.33092,-49.6957,2.25398,-1.11385 +04:BD:88:0D:DC:80,61.289,90.2208,4.0259,-35.0062,3.04417,-2.63175 +04:BD:88:50:38:30,121.912,71.8626,-1.78283,-49.9971,2.26707,-9.99155 +04:BD:88:50:49:80,84.2751,25.0538,1.49222,-40.4254,2.83503,-7.602 +04:BD:88:0D:6E:F0,120.821,61.1164,13.5875,-38.2404,3.44374,-2.61839 +04:BD:88:50:40:60,129.763,12.3819,7.93683,-35.1282,3.06737,-4.26669 +04:BD:88:0D:A5:E0,135.237,28.245,4.02208,-49.9997,2.50191,-2.87061 +24:DE:C6:61:B7:00,67.1349,74.3687,-9.99796,-49.156,2.35183,-0.89355 +04:BD:88:0D:A5:F0,134.781,29.0858,4.00298,-43.1381,2.48746,-6.84854 +04:BD:88:50:4D:A0,90.3609,16.8601,2.3904,-35.008,3.47571,-5.64231 +00:07:40:8B:7A:30,42.0218,92.7284,5.43893,-49.9913,2.35867,-0.201366 +24:DE:C6:61:B7:C0,66.3967,63.7506,-0.250435,-49.9809,2.04389,-3.82128 +70:73:CB:B6:58:30,80.3016,18.3201,10.8602,-49.9581,2.84485,-5.15359 +24:DE:C6:61:B7:D0,105.209,52.0633,-0.350371,-35.0012,2.28654,-8.43184 +04:BD:88:50:4B:80,89.3476,3.92014,1.30161,-35.0026,3.23554,-6.60991 +04:BD:88:0D:AC:30,-4.2106,22.489,-9.56091,-49.979,1.45268,-4.58033 +24:DE:C6:61:B8:10,88.1564,35.3913,0.342312,-35.0049,2.77558,-8.7812 +04:BD:88:0D:D8:40,61.9292,11.7163,0.744604,-44.5367,3.05007,-1.24771 +04:BD:88:0D:5B:B0,79.5373,22.6322,10.8227,-40.7107,3.11971,-8.14276 +04:BD:88:0D:95:90,79.0245,22.721,13.0689,-40.4332,3.49855,-3.18497 +04:BD:88:50:4D:B0,90.1575,17.3069,-0.446496,-35.0196,3.08851,-5.95266 +04:BD:88:50:40:70,129.27,13.4976,8.12434,-37.7665,2.42516,-7.36279 +04:BD:88:50:40:00,131.354,15.5469,5.16526,-49.6713,2.26854,-5.5807 +00:90:4C:49:00:20,38.3657,91.0812,-12.2644,-49.9936,2.19976,-0.636317 +00:07:40:9F:14:70,49.965,-38.0442,-2.40538,-49.9979,2.07355,-0.56936 +04:BD:88:0D:B4:60,76.7,68.0342,-6.78332,-49.9848,2.61085,-1.2733 +04:BD:88:0D:F6:20,70.1452,63.9766,-10.3678,-49.9798,2.55316,-0.902064 +04:BD:88:0D:F7:E0,63.8286,87.5842,-2.79041,-49.9764,2.29822,-0.000814016 +04:BD:88:50:46:50,66.9395,18.0641,1.3915,-42.7413,3.32566,-0.433864 +04:BD:88:50:40:10,131.763,14.5722,6.305,-49.2889,2.42529,-7.48766 +04:BD:88:0D:D9:50,80.3704,92.1215,25.988,-49.9934,1.19493,-5.75634 +64:70:02:92:13:00,-0.692318,91.5478,10.4957,-49.9347,2.05727,-0.636378 +04:BD:88:0D:95:80,78.7645,21.1212,13.9495,-37.8095,3.42004,-2.10601 +04:BD:88:50:4D:40,68.7541,13.8713,12.0002,-47.6747,1.44208,-6.63998 +5C:96:9D:66:34:30,34.4218,-73.6812,9.68786,-38.6282,2.58137,-0.543019 +04:BD:88:0D:98:00,11.1829,53.3534,-8.10652,-47.8203,2.11338,-0.726475 +04:BD:88:0D:5F:70,88.8169,34.2218,5.06589,-42.3359,2.17583,-11.7357 +04:BD:88:0D:BF:10,55.7389,-36.1212,-11.0948,-49.9826,1.61311,-2.81289 +04:BD:88:0D:D9:40,71.9179,91.9191,4.01933,-49.9987,2.00224,-5.32896 +00:C0:CA:7D:C3:60,170.896,2.97641,1.13119,-49.886,2.10314,-0.154738 +84:51:81:8B:B7:E0,120.723,53.3081,4.97545,-49.9991,2.61727,-6.60546 +00:18:39:D3:AF:00,97.0356,16.5325,10.2935,-49.1876,2.7465,-1.11597 +04:BD:88:0D:BF:00,78.559,60.5497,-7.47801,-49.7485,2.49862,-1.83014 +70:73:CB:B6:58:40,78.278,20.7332,9.84987,-43.6191,3.46317,-6.18348 +04:BD:88:0D:C0:A0,119.491,45.2099,14.1613,-41.7541,3.49007,-6.92284 +04:BD:88:0D:D0:30,96.6917,22.6303,9.89707,-43.1401,2.83267,-6.64655 +04:BD:88:0D:F6:30,10.6477,56.5952,-19.7491,-39.0774,2.4628,-1.19393 +04:BD:88:0D:F7:F0,177.434,93.6494,-1.40187,-49.9772,1.74344,-1.44321 +04:BD:88:0D:B4:70,77.7908,68.0696,-7.78729,-48.9825,2.32418,-6.59958 +04:BD:88:50:3F:E0,69.6372,60.0201,-7.93029,-49.9863,2.69515,-0.0876285 +04:BD:88:0D:59:D0,123.321,5.98932,19.0209,-35.0047,2.74303,-4.72328 +04:BD:88:0E:0F:50,95.6385,21.351,14.9545,-38.7529,3.22616,-6.58033 +04:BD:88:50:49:90,85.4624,25.1076,0.0974095,-48.9582,2.47627,-6.40795 +00:1F:F3:C4:D6:80,86.3851,37.4918,14.3314,-48.0705,2.83077,-2.39407 +04:BD:88:50:46:40,66.9296,17.4863,2.02436,-37.7021,3.43908,-1.70041 +C0:C1:C0:18:4D:10,64.5807,62.2852,9.99459,-37.7211,3.33022,-0.0117366 +00:1D:73:DE:30:10,31.7746,-25.6241,25.3286,-49.9877,2.04809,-0.467863 +5C:96:9D:65:B1:60,50.3786,-12.1958,1.09019,-46.5651,2.49526,-0.735329 +04:BD:88:0D:DB:70,71.6713,67.0077,1.47469,-35.0013,3.05828,-1.90065 +04:BD:88:0D:C0:B0,119.157,45.3078,13.9462,-36.0302,3.49884,-7.89359 +24:DE:C6:61:B6:A0,68.0411,69.5789,-0.983092,-47.9711,2.38076,-1.05341 +04:BD:88:50:4B:B0,83.5287,11.165,-0.874307,-35.0013,2.8947,-7.73954 +58:6D:8F:40:17:50,118.001,46.8434,7.226,-46.4102,2.61688,-7.25978 +04:BD:88:0E:0F:40,95.3664,21.5758,13.1468,-43.7529,2.93255,-2.75834 +04:BD:88:0D:59:C0,139.1,2.52001,11.9983,-35.0033,2.74426,-5.78839 +04:BD:88:0D:CD:B0,90.3253,24.1403,6.40912,-46.2239,2.95668,-7.76077 +28:CF:DA:B4:A4:E0,71.3841,17.3987,10.9198,-41.6981,3.49771,-2.76056 +04:BD:88:0D:77:A0,128.963,11.7249,0.270389,-35.0045,3.02279,-7.00728 +04:BD:88:0D:56:C0,86.07,18.664,4.00711,-37.366,3.33351,-5.39645 +28:CF:DA:B4:A4:F0,75.1324,16.4766,9.31253,-37.9139,3.49779,-6.13855 +04:BD:88:0D:77:B0,127.311,12.5711,-0.192137,-35.007,2.65978,-8.72069 +04:BD:88:0D:56:D0,86.8781,19.3035,4.42533,-48.3854,2.65331,-7.36201 +04:BD:88:0D:D0:20,96.158,22.032,10.5687,-36.192,3.26348,-5.34901 +04:BD:88:0D:AC:20,73.0754,64.3254,-7.0126,-44.5641,2.77401,-1.90436 +00:12:17:49:7E:00,75.4399,-1.60531,34.4457,-36.5552,3.1258,-1.1407 +04:BD:88:0D:D9:A0,101.618,41.7706,11.2442,-36.6078,3.03085,-5.12036 +04:BD:88:0D:CD:A0,90.3729,22.0323,4.27899,-40.6529,2.99745,-5.85436 +04:BD:88:0D:D9:B0,101.345,44.3782,11.9903,-40.6434,2.56346,-8.32296 +00:14:BF:B2:32:90,82.2126,2.34229,11.9962,-43.0968,2.86363,-2.2501 +04:BD:88:0D:D0:90,117.927,45.0786,10.1503,-40.8122,2.92621,-8.37503 +04:BD:88:0D:5B:A0,79.3198,21.7946,10.245,-42.0873,2.99351,-4.19679 +04:BD:88:0D:6C:F0,108.571,54.5004,4.25067,-45.2783,2.15997,-10.3004 +04:BD:88:50:4E:70,117.341,54.2166,-0.025421,-41.0344,2.5103,-5.99929 +32:CD:A7:94:50:30,123.023,44.722,9.34927,-49.9657,2.85353,-3.46767 +04:BD:88:0D:F1:80,64.0574,16.8605,8.00059,-49.98,2.8813,-0.81782 +F4:EC:38:BA:9F:00,85.0672,43.6407,4.00242,-43.7068,2.74683,-6.03561 +04:BD:88:0D:5F:60,88.3708,35.3684,4.16747,-39.8495,2.42562,-11.3923 +38:2C:4A:67:E7:E0,73.7221,66.6399,-6.41874,-45.0438,2.76172,-0.207669 +04:BD:88:0D:6C:E0,108.264,52.6367,4.27121,-39.1143,2.67789,-8.81927 +04:BD:88:50:46:80,113.13,171.087,4.05088,-35.0641,2.39484,-7.82784 +24:DE:C6:61:B5:60,84.6697,86.3409,-7.22591,-35.0078,2.67631,-4.07704 +04:BD:88:0D:DB:60,70.4842,66.2288,-2.97031,-35.0111,3.16627,-0.953721 +04:BD:88:50:4B:90,93.0154,7.40264,-0.265897,-36.6803,2.55107,-8.24377 +04:BD:88:0D:C3:00,118.044,9.02218,5.36992,-46.0305,2.56256,-2.13349 +04:BD:88:0D:C4:C0,120.738,62.6373,10.2858,-37.6953,3.35918,-3.82192 +04:BD:88:0D:DC:90,41.1354,117.319,6.7466,-35.0025,2.63455,-2.74291 +04:BD:88:0D:99:10,103.997,105.754,12.3157,-49.1102,2.33931,-0.190116 +04:BD:88:50:4B:A0,79.0666,12.1056,0.821342,-35.0021,3.35455,-5.52112 +04:BD:88:0D:C4:D0,118.331,63.0411,10.267,-35.4286,3.48023,-6.31238 +00:13:10:92:8E:C0,71.949,16.5233,12.0261,-40.8699,3.49904,-1.18502 +04:BD:88:0D:C3:10,116.698,9.70811,8.00064,-42.1061,2.32084,-5.88028 diff --git a/competition/src/competition/data/uji-ti/wifiParams.txt b/competition/src/competition/data/uji-ti/wifiParams.txt new file mode 100644 index 0000000..6bd482d --- /dev/null +++ b/competition/src/competition/data/uji-ti/wifiParams.txt @@ -0,0 +1,60 @@ +00:13:C3:44:D6:E0,18.0702,76.4523,10.549,-35.0014,2.93199,-6.27181 +06:27:22:0B:36:30,40.1241,47.7622,21.7446,-49.9913,2.44201,-6.72851 +00:1A:A1:5C:F9:60,29.439,69.7044,6.73497,-35.0024,3.00487,-4.59787 +00:13:C3:44:D8:E0,45.7304,75.6823,10.4813,-35.0008,2.71693,-9.96737 +00:14:69:F2:39:C0,15.9948,74.5817,3.99626,-49.9958,1.44476,-10.4591 +00:13:C3:44:B0:70,31.7924,45.3754,10.553,-37.1094,3.13311,-6.59345 +8C:0C:A3:29:7D:60,14.653,72.9748,3.99988,-35.0888,1.29455,-15.65 +90:F6:52:FF:CD:70,33.9192,76.0892,4.00254,-35.0109,3.37856,-5.2131 +00:12:17:DE:2B:00,-38.7018,5.33146,11.1404,-49.9991,1.78626,-1.67534 +06:27:22:0B:C4:60,11.9001,10.4882,10.0396,-36.3239,2.7002,-6.14787 +2A:A4:3C:04:99:A0,5.2489,33.3503,-7.11783,-35.0009,3.18547,-0.916387 +F8:1E:DF:FA:D1:D0,16.3958,24.5382,17.8596,-39.2878,3.09384,-5.82037 +00:27:22:0B:C4:60,12.3254,10.9224,9.55549,-35.0005,2.82285,-7.5039 +00:14:1C:DD:F3:40,50.4348,12.935,15.6604,-43.4289,2.27774,-6.98219 +0A:18:D6:04:29:60,123.026,46.6534,-4.1658,-49.9872,1.88507,-0.226678 +00:1A:A1:5C:F6:50,117.809,25.2796,3.56561,-35.0019,2.41297,-1.42737 +04:18:D6:04:26:40,19.0649,-0.893982,-4.02479,-48.2036,1.9101,-2.76744 +00:27:22:0B:36:30,34.5967,49.8676,19.958,-47.9794,2.9979,-6.47409 +C0:C1:C0:5A:A0:50,33.3971,21.3792,7.56039,-44.2523,2.71674,-3.47578 +00:15:6D:E4:7D:10,33.3023,95.0565,15.3638,-35.0018,3.22822,-1.75908 +00:13:C3:44:D9:B0,19.4742,14.0811,4.00824,-35.0073,2.86109,-7.32064 +D4:C9:EF:54:65:40,30.8112,16.7518,17.7033,-35.0033,3.2996,-3.78139 +84:1B:5E:3F:88:60,46.7823,75.066,4.72317,-49.9985,2.00389,-4.43731 +80:49:71:16:5E:C0,33.4434,36.517,8.91344,-49.6736,2.52377,-4.82025 +00:27:22:4A:1F:60,20.2303,122.245,22.5083,-49.9956,1.84772,-0.463619 +00:1D:A2:C9:27:50,-2.9687,47.6667,5.1596,-38.9358,2.87797,-0.757473 +00:14:1C:DD:F6:70,16.4238,14.1287,15.5029,-35.0018,2.84835,-6.56431 +00:27:22:0B:C4:30,29.5108,45.6983,20.0469,-49.9946,2.21772,-6.35757 +00:90:4C:5F:00:20,22.9083,18.3212,10.9642,-49.9856,2.33844,-6.80419 +00:14:1C:DD:F1:E0,16.5423,75.5931,15.5193,-35.0007,2.88951,-7.85462 +02:CA:FF:EE:BA:B0,-27.1346,-9.59034,17.5316,-49.937,1.90515,-1.25176 +C8:BC:C8:C1:95:50,53.5559,119.893,8.55838,-49.9982,1.87476,-2.08567 +C8:D7:19:09:37:80,26.8558,92.7132,4.52568,-35.0011,3.08478,-5.42009 +00:14:1C:DD:FA:A0,34.365,45.422,15.0598,-35.0073,3.03597,-7.88405 +00:13:C3:44:EE:F0,22.4794,17.9141,9.03903,-35.0007,2.93753,-5.31065 +00:17:59:FB:19:50,48.0488,76.0151,15.3844,-44.9409,2.07276,-6.31787 +00:14:1C:DD:FC:40,32.9535,68.3393,12.1693,-35.0004,2.80755,-4.49933 +06:27:22:0B:C4:30,22.8095,43.7261,12.2014,-44.9756,2.76435,-6.04689 +00:27:22:0B:C4:20,-49.7517,111.983,3.14062,-49.997,1.41224,-3.29481 +06:25:BC:8A:8D:80,39.6838,18.0742,4.00581,-35.0322,3.27146,-4.73644 +00:14:69:F2:3B:80,24.188,13.5991,1.95709,-35.0005,2.9343,-6.53056 +00:25:BC:8A:8D:80,39.345,17.8098,4.00397,-35.0139,3.24465,-4.6729 +46:33:4C:34:96:E0,26.1628,24.6621,2.45785,-49.967,2.30937,-3.2212 +74:EA:3A:A0:52:80,51.5625,11.8496,0.275973,-35.0084,3.28797,-4.57739 +C2:9F:DB:1C:91:A0,-44.4874,28.7275,11.6671,-42.7231,2.23332,-1.9024 +06:27:22:0B:C4:20,92.5088,94.0343,-5.20496,-49.9992,1.62169,-2.46994 +00:13:C3:44:D6:70,46.6571,13.3471,10.2139,-35.003,2.93862,-11.0769 +00:14:69:F2:35:50,31.7231,43.772,3.77775,-39.4755,2.80505,-5.33444 +00:1A:6D:9B:9A:20,40.2208,71.44,4.29996,-35.0008,3.13437,-4.57072 +00:13:C3:44:B5:60,46.2441,13.2052,4.01291,-35.0053,2.72518,-7.25024 +00:19:56:AC:7B:00,35.2686,22.2504,6.86353,-35.0018,3.0237,-4.76125 +00:13:80:43:D1:50,33.9398,20.2003,12.0034,-35.0036,3.02282,-2.67427 +00:14:69:F2:31:40,31.0475,84.9883,4.22095,-42.0396,2.75453,-6.99953 +00:13:C3:44:C7:20,33.6595,41.0793,7.10509,-36.1616,2.59204,-8.10245 +0A:18:D6:04:25:C0,75.1088,1.41437,-5.43817,-49.9996,1.69289,-1.80604 +00:14:1C:DE:93:A0,44.9431,14.5584,-2.87313,-39.2836,2.85206,-6.0478 +04:18:D6:04:25:C0,53.4704,25.9522,1.20447,-49.9989,2.14101,-1.20614 +00:27:22:0B:35:80,32.3521,52.3023,3.99926,-48.3477,1.42975,-9.12723 +00:13:C3:44:D8:20,17.0173,76.4053,4.28259,-35.0038,2.74942,-10.1883 +06:27:22:0B:35:80,32.5252,54.1553,3.05857,-49.9546,1.41625,-8.55129 diff --git a/competition/src/competition/data/uji-ub/wifiParams.txt b/competition/src/competition/data/uji-ub/wifiParams.txt new file mode 100644 index 0000000..b9ee073 --- /dev/null +++ b/competition/src/competition/data/uji-ub/wifiParams.txt @@ -0,0 +1,101 @@ +FF:FF:FF:FF:00:30,61.3255,49.1117,12.8065,-35.0008,2.5384,-6.55057 +FF:FF:FF:FF:01:29,29.8202,26.1079,20.1286,-49.9995,2.82248,-0.145847 +FF:FF:FF:FF:00:68,51.0567,18.1168,9.0434,-35.0187,3.49819,-9.93346 +FF:FF:FF:FF:01:61,61.5426,26.656,5.23509,-35.0278,3.49998,-10.5933 +FF:FF:FF:FF:01:13,50.8348,19.6734,13.0748,-45.0079,3.49432,-1.70386 +FF:FF:FF:FF:00:87,58.733,8.075,5.03149,-35.9899,3.49952,-10.8467 +FF:FF:FF:FF:00:26,23.6657,16.9525,5.25899,-35,3.29068,-9.69726 +FF:FF:FF:FF:00:64,40.4603,-4.60327,12.0001,-35.007,3.30027,-2.15189 +FF:FF:FF:FF:01:67,37.2653,18.5164,16.5336,-49.3521,3.44172,-1.76764 +FF:FF:FF:FF:01:06,44.6022,-5.20636,12.0101,-35.1878,3.49929,-0.343725 +FF:FF:FF:FF:01:34,41.6675,22.512,9.73085,-49.9978,3.4997,-4.30721 +FF:FF:FF:FF:00:17,48.8877,-13.306,16.0022,-36.1515,2.92935,-2.45048 +FF:FF:FF:FF:01:10,29.7338,26.4258,23.9674,-38.9589,3.49963,-5.43652 +FF:FF:FF:FF:01:71,40.2638,28.4951,22.9366,-38.9438,2.23165,-14.6617 +FF:FF:FF:FF:00:34,75.3005,29.2538,3.99767,-35.0035,3.09486,-0.832926 +FF:FF:FF:FF:01:65,31.5907,43.052,19.9942,-47.3626,2.43991,-2.29432 +FF:FF:FF:FF:00:90,-61.1276,22.543,20.7044,-49.8111,1.75582,-0.337521 +FF:FF:FF:FF:00:16,44.8759,-8.18405,16.0004,-35.0026,3.48937,-1.28285 +FF:FF:FF:FF:00:45,37.3255,15.7237,23.1597,-49.9664,3.45164,-2.89579 +FF:FF:FF:FF:01:21,50.092,26.9431,21.8817,-35.0372,3.49929,-6.15205 +FF:FF:FF:FF:01:51,55.6082,24.6096,19.1434,-35.0004,3.09284,-4.76922 +FF:FF:FF:FF:01:22,33.5701,26.5572,22.695,-35.0033,2.39651,-14.9836 +FF:FF:FF:FF:00:46,51.9681,8.92208,4.93606,-35.0062,3.44907,-11.4138 +FF:FF:FF:FF:00:51,55.6536,33.4605,3.99827,-36.1021,3.38589,-1.08473 +FF:FF:FF:FF:01:19,42.1772,3.33725,8.05093,-36.2824,3.49993,-8.39715 +FF:FF:FF:FF:01:48,33.5646,26.6701,22.3995,-35.0039,2.25191,-15.7439 +FF:FF:FF:FF:00:99,51.6486,28.6696,15.9935,-49.5486,3.0511,-0.584962 +FF:FF:FF:FF:01:14,52.3689,26.4285,18.5044,-36.3501,3.49948,-5.60479 +FF:FF:FF:FF:01:23,30.3933,26.5188,22.0665,-45.7997,2.68648,-6.73393 +FF:FF:FF:FF:01:40,52.0377,26.4407,24.2133,-41.5633,3.49894,-5.42151 +FF:FF:FF:FF:00:47,24.3982,16.974,5.28612,-35.0009,3.33422,-8.67 +FF:FF:FF:FF:01:57,20.0917,-1.92616,20.0011,-35.0084,3.3683,-1.02511 +FF:FF:FF:FF:01:45,43.3059,1.44429,8.00549,-35.0176,3.49998,-9.14345 +FF:FF:FF:FF:00:08,47.6738,21.7745,4.80425,-45.169,2.75624,-1.76925 +FF:FF:FF:FF:01:16,31.5916,23.5406,15.9988,-46.5109,2.52192,-5.85123 +FF:FF:FF:FF:01:44,59.6713,24.2997,26.3772,-38.4904,3.49907,-2.44965 +FF:FF:FF:FF:00:07,39.9031,45.1725,15.9918,-46.5745,2.96254,-0.195176 +FF:FF:FF:FF:01:15,33.816,26.6505,24.6709,-35.0411,3.34374,-10.7747 +FF:FF:FF:FF:00:24,40.5597,16.8742,25.9081,-49.9799,3.49916,-0.00550805 +FF:FF:FF:FF:01:79,55.6672,23.8436,4.01151,-42.32,3.40306,-7.73111 +FF:FF:FF:FF:01:18,60.4541,23.5074,28.0572,-37.1776,3.49948,-3.20076 +FF:FF:FF:FF:00:43,15.3138,-44.063,9.83808,-41.7184,2.43117,-0.00026139 +FF:FF:FF:FF:01:69,21.7512,-2.37511,-19.6113,-49.9837,2.15097,-0.000342212 +FF:FF:FF:FF:01:08,15.4733,16.3318,16.0031,-44.95,2.61319,-6.05641 +FF:FF:FF:FF:01:27,1.36324,34.7171,20.1489,-35.1078,2.8743,-0.423486 +FF:FF:FF:FF:00:94,63.1968,25.996,5.2847,-35.9412,3.49993,-3.70427 +FF:FF:FF:FF:01:70,35.4791,28.4217,23.0721,-35.0009,2.93582,-11.7367 +FF:FF:FF:FF:01:35,41.9945,22.5251,10.5587,-49.9896,3.4996,-4.97646 +FF:FF:FF:FF:01:64,37.161,48.9912,21.0677,-35.8962,3.49994,-8.10018 +FF:FF:FF:FF:01:25,55.2976,24.9868,19.5005,-35.0035,2.96912,-6.85054 +FF:FF:FF:FF:01:47,50.1213,27.3851,22.7135,-35.0086,3.43343,-5.27027 +FF:FF:FF:FF:00:21,42.6089,15.8034,23.7548,-49.9977,2.67163,-4.30658 +FF:FF:FF:FF:00:50,71.9575,40.3268,15.9927,-49.9863,1.35871,-9.15773 +FF:FF:FF:FF:01:49,30.3275,26.4448,21.4412,-47.6343,2.41318,-8.09468 +FF:FF:FF:FF:00:71,0.831536,-5.34977,20.0126,-43.6724,2.32186,-1.12729 +FF:FF:FF:FF:00:13,40.1764,-30.8187,20.0027,-35.004,2.84513,-1.004 +FF:FF:FF:FF:01:68,41.3841,9.60216,20.0002,-49.9846,3.49977,-2.62543 +FF:FF:FF:FF:00:04,29.2682,-15.2735,16.0026,-35.038,2.99774,-3.51901 +FF:FF:FF:FF:00:65,43.7648,27.4297,8.00249,-35.0015,2.77887,-9.44508 +FF:FF:FF:FF:00:35,46.6445,71.8687,12.0157,-35.0196,2.79189,-2.1425 +FF:FF:FF:FF:01:72,30.0851,28.9715,21.5068,-35.0002,2.58584,-6.1347 +FF:FF:FF:FF:00:96,63.5721,26.041,5.31246,-38.328,3.49978,-1.36387 +FF:FF:FF:FF:01:11,35.7941,30.3036,25.3933,-42.6625,3.06023,-5.5328 +FF:FF:FF:FF:00:10,39.0899,55.4821,15.9913,-45.5138,2.66811,-1.61757 +FF:FF:FF:FF:01:09,21.337,18.7888,16.9388,-49.9198,2.62881,-3.8303 +FF:FF:FF:FF:01:05,26.0588,18.4117,16.0091,-47.9079,2.48883,-2.45525 +FF:FF:FF:FF:00:93,30.2106,63.7146,16.4056,-35.0029,2.95634,-2.04168 +FF:FF:FF:FF:00:60,-68.2546,41.3555,19.3967,-48.3603,1.91815,-1.82882 +FF:FF:FF:FF:00:09,47.6367,60.1411,15.9965,-36.826,3.00483,-1.52924 +FF:FF:FF:FF:01:46,51.2882,22.3053,9.20142,-35.0003,3.39486,-9.84389 +FF:FF:FF:FF:01:73,54.1249,16.4094,4.27109,-43.3027,3.49996,-7.77886 +FF:FF:FF:FF:00:36,47.2464,50.9547,16.0034,-35.0008,3.42789,-0.153835 +FF:FF:FF:FF:00:56,37.9049,2.95165,8.00579,-35.0458,2.97445,-8.98925 +FF:FF:FF:FF:01:26,43.5824,2.79313,8.00019,-35.0126,2.84525,-11.329 +FF:FF:FF:FF:01:60,47.757,35.0984,11.9932,-45.6897,1.1362,-9.47875 +FF:FF:FF:FF:00:67,37.9418,3.57521,8.00547,-35.0106,2.99568,-9.18844 +FF:FF:FF:FF:01:43,49.171,27.3086,11.9517,-35.0414,3.49992,-8.14544 +FF:FF:FF:FF:00:25,51.5967,8.96885,4.82255,-35.0012,3.44641,-12.7841 +FF:FF:FF:FF:01:62,61.8046,24.6624,5.34482,-37.4709,3.49995,-7.51762 +FF:FF:FF:FF:00:12,36.1314,7.31808,4.00517,-35.0422,2.96786,-14.3049 +FF:FF:FF:FF:00:52,46.1982,57.9144,15.9848,-35.0031,2.92728,-2.89102 +FF:FF:FF:FF:00:49,55.5027,60.6269,15.9932,-35.0023,3.18631,-1.20988 +FF:FF:FF:FF:01:42,31.8036,23.5235,15.9984,-46.8514,2.77668,-4.5452 +FF:FF:FF:FF:00:66,42.0519,21.9564,-0.00522796,-35.0022,1.8355,-10.5922 +FF:FF:FF:FF:00:05,33.5066,31.3112,8.96058,-37.2085,3.4105,-3.26688 +FF:FF:FF:FF:01:30,57.3403,24.1377,5.40282,-37.1963,3.49977,-10.0038 +FF:FF:FF:FF:00:54,44.1486,28.1293,8.00134,-35.0275,2.7215,-9.59048 +FF:FF:FF:FF:01:52,46.018,2.23849,8.00049,-35.0048,2.82059,-10.6605 +FF:FF:FF:FF:01:17,47.533,24.2954,11.9735,-38.7352,3.49961,-5.3735 +FF:FF:FF:FF:00:55,42.4996,22.5314,-3.82074e-05,-35.0041,1.86698,-10.089 +FF:FF:FF:FF:01:31,34.5379,31.231,19.145,-41.7621,3.21528,-0.00058413 +FF:FF:FF:FF:01:41,33.9805,26.6361,24.6389,-35.4263,3.31241,-7.56465 +FF:FF:FF:FF:00:48,51.1195,24.8872,3.94769,-37.5513,3.49986,-7.68639 +FF:FF:FF:FF:01:24,46.3338,28.237,8.00239,-35.0015,2.79479,-10.0347 +FF:FF:FF:FF:00:27,50.9023,24.6375,3.76086,-39.2458,3.4995,-6.92521 +FF:FF:FF:FF:01:20,51.1002,22.0841,9.14745,-35.0026,3.41654,-8.99681 +FF:FF:FF:FF:00:40,30.3214,-10.3777,16.0046,-48.4109,2.48731,-2.01044 +FF:FF:FF:FF:01:39,49.9374,19.2717,12.9782,-46.553,3.44212,-1.22435 +FF:FF:FF:FF:00:53,39.4442,49.9108,12.2648,-35.011,3.42754,-0.000203297 +FF:FF:FF:FF:01:50,47.1049,28.1215,8.00045,-35.0063,2.77865,-9.59834 +FF:FF:FF:FF:00:57,51.1749,18.5591,9.48191,-35.212,3.49933,-9.38768 diff --git a/competition/src/competition/filter/Logic.h b/competition/src/competition/filter/Logic.h index d677f29..d75af8c 100644 --- a/competition/src/competition/filter/Logic.h +++ b/competition/src/competition/filter/Logic.h @@ -13,19 +13,27 @@ #include #include #include +#include #include +#include + #include #include "WiFi.h" #include "../Scaler.h" #include "Structs.h" +#include //struct MyWalkState : public WalkState, public WalkStateHeading, public WalkStateRelativePressure { // MyWalkState(const GridPoint& gp, const Heading head, const float relativePressure) : WalkState(gp), WalkStateHeading(head, 0), WalkStateRelativePressure(relativePressure) {;} //}; +const bool barometer = true; + struct MyNode : public GridPoint, public GridNode, public GridNodeImportance { + + MyNode() : GridPoint() {;} MyNode(const int x, const int y, const int z) : GridPoint(x,y,z) {;} }; @@ -104,40 +112,71 @@ struct PFTrans : public K::ParticleFilterTransition { WalkModuleSpread modSpread; WalkModuleFavorZ modFavorZ; WalkModuleButterActivity modButterAct; + WalkModuleButterActivityPercent modButterActPerc; //WalkModulePreventVisited modPreventVisited; - PFTrans(Grid& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, 4.5f) {//, modPressure(ctrl, 0.100) { + PFTrans(Grid& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, 4.5f), modButterActPerc(ctrl) {//, modPressure(ctrl, 0.100) { - walker.addModule(&modHead); - //walker.addModule(&modSpread); // might help in some situations! keep in mind! + walker.addModule(&modHead); + //walker.addModule(&modSpread); // might help in some situations! keep in mind! - //walker.addModule(&modHeadUgly); - walker.addModule(&modImportance); - walker.addModule(&modFavorZ); - walker.addModule(&modButterAct); - //walker.addModule(&modWifi); - //walker.addModule(&modPreventVisited); + //walker.addModule(&modHeadUgly); + walker.addModule(&modImportance); + walker.addModule(&modFavorZ); + //walker.addModule(&modButterAct); + if(barometer){ + walker.addModule(&modButterActPerc); + } + //walker.addModule(&modWifi); + //walker.addModule(&modPreventVisited); } virtual void transition(std::vector>& particles, const MyControl* control) override { - Distribution::Normal distStep(1, 0.15); - - float tmpSum1 = 0; - float tmpSum2 = 0; - int tmpCnt = 0; + Distribution::Normal distStep(1, 0.35); + Distribution::Normal distUpDown(1, 0.5); for (K::Particle& p : particles) { - tmpSum1 += p.state.relPres; + p.state.positionOld = p.state.position; //GridPoint(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm); + const float var = distStep.draw(); - const float dist = control->numSteps * var * 0.76; // 75cm + variance for every detected step + const float varUpDown = distUpDown.draw(); + float dist = control->numSteps * var * 0.78; // 75cm + variance for every detected step + + + MyNode node = grid.getNodeFor(p.state.position); + + + if(barometer){ + /** Hier steckt noch viel Optimierungspotential drinnen! Richtige Distanz für den richtigen Bereich.*/ + if(node.getType() == 2){ + //wenn die wahrscheinlichkeit vom aufzug gering, dann sollen die partikel die noch drin sind auch stehen bleiben. + + //TODO: varianz der erkennung eines fahrstuhls kleiner machen. ggf. die andere methode mit "harten" grenzen... + dist = 3 * varUpDown * (control->barometer.actProbs.elevatorDown > control->barometer.actProbs.elevatorUp ? control->barometer.actProbs.elevatorDown : control->barometer.actProbs.elevatorUp); //multiply with prob of elevator? + } + + /** stell sicher, das er die treppe läuft, auch wenn kein step erkannt wurde*/ + if(node.getType() == 1){ + dist = control->numSteps * 0.4 + 0.6; // * varUpDown * (control->barometer.actProbs.stairsDown > control->barometer.actProbs.stairsUp ? control->barometer.actProbs.stairsDown : control->barometer.actProbs.stairsUp); //* wahrscheinlichkeit treppe + } + } + + //we activity is stairchange, but particle is not changing, set dist to zero +// if(control->barometer.act == ActivityButterPressure::Activity::DOWN || control->barometer.act == ActivityButterPressure::Activity::DOWN){ +// + // != type_stair + //float diff = p.state.positionOld.inMeter().z - p.state.position.inMeter().z; + +// if(diff == 0){ +// dist = 0.0; +// } +// } - //activity - p.state.act = control->barometer.act; // --------- OLD // transfer from state to walkstate @@ -154,8 +193,39 @@ struct PFTrans : public K::ParticleFilterTransition { // update the particle in-place p.state = walker.getDestination(grid, p.state, dist); - tmpSum2 += p.state.relPres; - ++tmpCnt; + // update the baromter + + //TODO: hier muss zwischen Aufzug und Treppe unterschieden werden. + if(control->barometer.resetRelPressure){ + p.state.hPa = 0.0; + } + float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z; + p.state.hPa += deltaZ_cm * 0.105f; + + + //which activity was performed by the particle? This is used in the smoothing procedure! + //walking updwards with node = stairs => stairs up + if(node.getType() == 1){ + if(deltaZ_cm < 0.0f){MyState::Activity::STAIRS_UP;} + if(deltaZ_cm > 0.0f){MyState::Activity::STAIRS_DOWN;} + } + //same for elevators + if(node.getType() == 2){ + if(deltaZ_cm < 0.0f){MyState::Activity::ELEVATOR_UP;} + if(deltaZ_cm > 0.0f){MyState::Activity::ELEVATOR_DOWN;} + } + //no step = no walking. + else if(control->numSteps == 0){ + p.state.act = MyState::Activity::STANDING; + } + //walking is the only option left + else{ + p.state.act = MyState::Activity::WALKING; + } + + //save the measured activities + p.state.refActProbs = control->barometer.actProbs; + p.state.refTurnAngle = control->turnAngle; } @@ -166,18 +236,236 @@ struct PFTrans : public K::ParticleFilterTransition { }; +/** + * smoothing transition starting at T with t, t-1,...0 + * @param particles_new p_t (Forward Filter) p2 + * @param particles_old p_t+1 (Smoothed Particles from Step before) p1 + * q(p1 | p2) is calculated + */ +struct BFTrans : public K::BackwardFilterTransition { + + Grid& grid; + + BFTrans(Grid& grid) : grid(grid) {;} + + virtual std::vector> transition(std::vector>const& particles_new, + std::vector>const& particles_old ) override { + + std::vector> predictionProbabilities; + + + omp_set_dynamic(0); // Explicitly disable dynamic teams + omp_set_num_threads(7); + #pragma omp parallel for shared(predictionProbabilities) + for (int i = 0; i < particles_old.size(); ++i) { + std::vector innerVector; + auto p1 = &particles_old[i]; + + for(int j = 0; j < particles_new.size(); ++j){ + + auto p2 = &particles_new[j]; + + const double distance_m = p2->state.position.getDistanceInMeter(p1->state.position); + + double muDistance = 0.8; + double sigmaDistance = 0.5; + double muFloor = 0.0; + double sigmaFloor = 0.2; + + + //jedem partikel seine aktivity die er zum zeitpunt t ausgeführt hat speichern. + //wenn echte aktivität treppen laufen war, der partikel aber stehen gebliebn ist, wird dieser niedrig bewertet. + double actProb = 1.0; + switch (p2->state.act) { + case MyState::Activity::ELEVATOR_DOWN : + muDistance = 0.0; + sigmaDistance = 0.3; + + muFloor = 3.0; + sigmaFloor = 1.0; + + actProb = p1->state.refActProbs.elevatorDown; + break; + + case MyState::Activity::STAIRS_DOWN: + muDistance = 0.5; + sigmaDistance = 0.3; + + muFloor = 0.5; + + actProb = p1->state.refActProbs.stairsDown; + break; + + case MyState::Activity::STAIRS_UP: + muDistance = 0.4; + sigmaDistance = 0.2; + + muFloor = -0.5; + + actProb = p1->state.refActProbs.stairsUp; + break; + + case MyState::Activity::STANDING: + muDistance = 0.0; + sigmaDistance = 0.2; + + muFloor = 0.0; + + actProb = p1->state.refActProbs.stay; + break; + + case MyState::Activity::WALKING: + muDistance = 0.8; + sigmaDistance = 0.5; + + muFloor = 0.0; + + actProb = p1->state.refActProbs.stay; + break; + + case MyState::Activity::ELEVATOR_UP : + muDistance = 0.0; + sigmaDistance = 0.3; + + muFloor = -3.0; + sigmaFloor = 1.0; + + actProb = p1->state.refActProbs.elevatorUp; + break; + + default: + muDistance = 0.8; + sigmaDistance = 0.5; + + muFloor = 0.0; + + actProb = p1->state.refActProbs.stay; + break; + } + + const double distProb = Distribution::Normal::getProbability(muDistance, sigmaDistance, distance_m); + + // is the heading change similiar to the measurement? + // QUATSCH! Ich brauch das Heading zwischen Punkt p1 und p2 und nicht die differenz. + double diffRad = p2->state.heading.direction.getSignedDiff(p1->state.heading.direction); + double diffDeg = Angle::radToDeg(diffRad); + double angularChange = Angle::radToDeg(p2->state.refTurnAngle); + + const double headingProb = Distribution::Normal::getProbability(angularChange, 5.0, diffDeg); + + + //check how near we are to the measurement + double diffZ = (p2->state.position.z_cm - p1->state.position.z_cm) / 100.0; + const double floorProb = Distribution::Normal::getProbability(muFloor, sigmaFloor, diffZ); + + + //combine the probabilities + double prob = distProb * headingProb * floorProb * actProb; + innerVector.push_back(prob); + + + } + #pragma omp critical + predictionProbabilities.push_back(innerVector); + } + + return predictionProbabilities; + + } + +}; + struct PFEval : public K::ParticleFilterEvaluation { WiFiModelX model; Scaler scaler; + Grid& grid; - PFEval(Scaler scaler, WiFiModelX wifiModel) : scaler(scaler), model(wifiModel) { + PFEval(Scaler scaler, WiFiModelX wifiModel, Grid& grid) : scaler(scaler), model(wifiModel), grid(grid) { } /** probability for WIFI */ - inline double getWIFI(const MyObs& observation, const Point3 pos_m) const { - return model.getProbability(observation.wifis, pos_m, Timestamp::fromSec(observation.curTS)); + inline double getWIFI(const MyObs& observation, const Point3 pos_m) const { + +// if(observation.act == ActivityButterPressure::Activity::UP || observation.act == ActivityButterPressure::Activity::DOWN){ +// return 1.0; +// } + + return model.getProbability(observation.wifis, pos_m, Timestamp::fromSec(observation.curTS)); + + +// //get the probabiliy for different z values. rebase particel to this value +// //eventl. hier nur die z koordinate des aktuellen partikel nehmen und die position der estimation +// double prob_1_p = model.getProbability(observation.wifis, Point3(pos_m.x, pos_m.y, pos_m.z + 4), Timestamp::fromSec(observation.curTS)); +// double prob_05_p = model.getProbability(observation.wifis, Point3(pos_m.x, pos_m.y, pos_m.z + 2), Timestamp::fromSec(observation.curTS)); +// double prob_025_p = model.getProbability(observation.wifis, Point3(pos_m.x, pos_m.y, pos_m.z + 1), Timestamp::fromSec(observation.curTS)); +// double prob_0 = model.getProbability(observation.wifis, Point3(pos_m.x, pos_m.y, pos_m.z ), Timestamp::fromSec(observation.curTS)); +// double prob_025_n = model.getProbability(observation.wifis, Point3(pos_m.x, pos_m.y, pos_m.z - 1), Timestamp::fromSec(observation.curTS)); +// double prob_05_n = model.getProbability(observation.wifis, Point3(pos_m.x, pos_m.y, pos_m.z - 2), Timestamp::fromSec(observation.curTS)); +// double prob_1_n = model.getProbability(observation.wifis, Point3(pos_m.x, pos_m.y, pos_m.z - 4), Timestamp::fromSec(observation.curTS)); + + +// // if positiv -> upstairs (not like barometer ;)) +// double diff = pos_m.z - posOld_m.z; +// if(diff > 3){ +// return prob_1_p; +// } +// else if(diff > 1.5){ +// return prob_05_p; +// } +// else if(diff > 0.5){ +// return prob_025_p; +// } +// else if (diff > -0.5){ +// return prob_0; +// } +// else if (diff > -1.5){ +// return prob_025_n; +// } +// else if (diff > -3){ +// return prob_05_n; +// } +// else{ +// return prob_1_n; +// } + + + +// if(prob_1_p > prob_05_p && prob_1_p > prob_025_p && prob_1_p > prob_0 && prob_1_p > prob_025_n && prob_1_p > prob_05_n && prob_1_p > prob_1_n){ +// point.z_cm += 400; +// return prob_1_p; +// } +// else if(prob_1_n > prob_05_p && prob_1_n > prob_025_p && prob_1_n > prob_0 && prob_1_n > prob_025_n && prob_1_n > prob_05_n){ +// point.z_cm += 400; +// return prob_1_p; +// } + +// else if (prob_05_p > prob_025_p && prob_05_p > prob_0 && prob_05_p > prob_025_n && prob_05_p > prob_05_n && prob_05_p > prob_1_n){ +// point.z_cm += 50; +// return prob_05_p; +// } +// else if (prob_025_p > prob_0 && prob_025_p > prob_025_n && prob_025_p > prob_05_n && prob_025_p > prob_1_n){ +// point.z_cm += 25; +// return prob_025_p; +// } +// else if (prob_0 > prob_025_n && prob_0 > prob_05_n && prob_0 > prob_1_n){ +// point.z_cm += 0; +// return prob_0; +// } +// else if (prob_025_n > prob_05_n && prob_025_n > prob_1_n){ +// point.z_cm -= 25; +// return prob_025_n; +// } +// else if (prob_05_n > prob_1_n){ +// point.z_cm -= 50; +// return prob_05_n; +// } +// else{ +// point.z_cm -= 400; +// return prob_1_n; +// } + } /** probability for GPS */ @@ -196,11 +484,122 @@ struct PFEval : public K::ParticleFilterEvaluation { } + //Old just for non probabilistic activity + inline double getBaro(const MyObs& observation, const GridPoint point, const GridPoint pointOld) const{ + + MyNode node = grid.getNodeFor(point); + MyNode nodeOld = grid.getNodeFor(pointOld); + + float diff = nodeOld.z_cm - node.z_cm; + + if(observation.act == ActivityButterPressure::Activity::UP){ + if(node.getType() == MyNode::TYPE_STAIR || node.getType() == MyNode::TYPE_ELEVATOR){ + if(diff < 0.0f){ + return 0.79; + } + else{ + return 0.01; + } + }else{ + return 0.01; + } + } + else if(observation.act == ActivityButterPressure::Activity::DOWN){ + if(node.getType() == MyNode::TYPE_STAIR || node.getType() == MyNode::TYPE_ELEVATOR){ + if(diff > 0.0f){ + return 0.79; + } + else{ + return 0.01; + } + }else{ + return 0.01; + } + } + else{ + return 1.0; + } + + + } + + inline double getBaroPercent(const MyObs& observation, const GridPoint point, const GridPoint pointOld) const{ + + MyNode node = grid.getNodeFor(point); + MyNode nodeOld = grid.getNodeFor(pointOld); + + float deltaZ_cm = nodeOld.z_cm - node.z_cm; + + //floor and doors + if(node.getType() == 0 || node.getType() == 3){ + return observation.actProbs.stay; + } + + //stairs + if(node.getType() == 1){ + +// //Wenn partikel noch auf treppe, dann sollte das hier etwas höher sein, wenn aber nicht mehr... sehr sehr klein! +// if (observation.act == ActivityButterPressure::Activity::STAY){ +// return observation.actProbs.stay * 0.5; +// } + + if (deltaZ_cm > 0){return observation.actProbs.stairsDown;} + if (deltaZ_cm < 0){return observation.actProbs.stairsUp;} + + return 0.8 * observation.actProbs.stairsDown > observation.actProbs.stairsUp ? observation.actProbs.stairsDown : observation.actProbs.stairsUp; + + } + + //elevators + if(node.getType() == 2){ + +// //Wenn partikel noch im aufzug, dann sollte das hier etwas höher sein, wenn aber nicht mehr... sehr sehr klein! +// if (observation.act == ActivityButterPressure::Activity::STAY){ +// return observation.actProbs.stay * 0.5; +// } + + if (deltaZ_cm > 0){return observation.actProbs.elevatorDown;} + if (deltaZ_cm < 0){return observation.actProbs.elevatorUp;} + + return 0.8 * observation.actProbs.elevatorDown > observation.actProbs.elevatorUp ? observation.actProbs.elevatorDown : observation.actProbs.elevatorUp; + } + + return 1.0; + } + + inline double getBaroPressure(const MyObs& observation, const float hPa) const{ + + //TODO: would it be better to print only a distribution if the relPressure was reset? + + float diff = observation.hPa - observation.referencePressure; + + return Distribution::Normal::getProbability(hPa, 0.10, diff); + } + /** probability for ALL sensors */ - inline double getALL(const MyObs& observation, const Point3 pos_m) const { - const double pWifi = getWIFI(observation, pos_m); + inline double getALL(const MyObs& observation, GridPoint& point, const GridPoint pointOld, const float hPa) const { + + Point3 pos_m = point.inMeter(); + Point3 posOld_m = pointOld.inMeter(); + + const double pWifi = getWIFI(observation, pos_m); const double pGps = getGPS(observation, pos_m); - return pWifi*pGps; + if(!barometer){ + return pWifi*pGps; + } + + const double pBaro = getBaro(observation, point, pointOld); + const double pBaroPercent = getBaroPercent(observation, point, pointOld); + const double pBaroPressure = getBaroPressure(observation, hPa); + + _assertNotNAN(pBaroPercent, "pups"); + _assertNotNAN(pGps,"pups"); + _assertNotNAN(pWifi, "pups"); + _assertNot0(pBaroPressure,"pBaroPressure is null"); + _assertNot0(pBaroPercent,"pBaroPercent is null"); + + + return pWifi*pGps*pBaroPercent*pBaroPressure; } virtual double evaluation(std::vector>& particles, const MyObs& observation) override { @@ -209,13 +608,17 @@ struct PFEval : public K::ParticleFilterEvaluation { for (K::Particle& p : particles) { - const double prob = getALL(observation, p.state.position.inMeter()); + const double prob = getALL(observation, p.state.position, p.state.positionOld, p.state.hPa); - p.weight = prob; - sum += prob; + p.weight = prob; + sum += (prob); } + if(sum == 0.0){ + return 1.0; + } + return sum; } diff --git a/competition/src/competition/filter/Structs.h b/competition/src/competition/filter/Structs.h index 5550eae..13015b9 100644 --- a/competition/src/competition/filter/Structs.h +++ b/competition/src/competition/filter/Structs.h @@ -15,6 +15,7 @@ #include #include +#include struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpread, public WalkStateFavorZ { @@ -25,12 +26,20 @@ struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpre // CURRENTLY OBSOLETE (did not work as expected) float relPres; // current pressure relative to t0 - ActivityButterPressure::Activity act; + float hPa = 0; + GridPoint positionOld; - MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0) {;} + enum Activity {ELEVATOR_DOWN, STAIRS_DOWN, WALKING, STANDING, STAIRS_UP, ELEVATOR_UP}; - MyState(GridPoint pos) : WalkState(pos), WalkStateHeading(Heading(0), 0) {;} + Activity act = STANDING; + ActivityButterPressurePercent::ActivityProbabilities refActProbs = ActivityButterPressurePercent::ActivityProbabilities(0.01f, 0.01f, 0.96f, 0.01f, 0.01f); + + float refTurnAngle = 0; + + MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), hPa(0) {;} + + MyState(GridPoint pos) : WalkState(pos), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), hPa(0) {;} MyState& operator += (const MyState& o) { this->position += o.position; @@ -106,6 +115,8 @@ struct MyControl { // sensor type 3 ActivityButterPressure::Activity act = ActivityButterPressure::STAY; + ActivityButterPressurePercent::ActivityProbabilities actProbs = ActivityButterPressurePercent::ActivityProbabilities(0.01f, 0.01f, 0.96f, 0.01f, 0.01f); + bool resetRelPressure; } barometer; @@ -116,6 +127,10 @@ struct MyObs { float curTS; WiFiMeasurements wifis; GPS gps; + ActivityButterPressure::Activity act; + ActivityButterPressurePercent::ActivityProbabilities actProbs = ActivityButterPressurePercent::ActivityProbabilities(0.01f, 0.01f, 0.96f, 0.01f, 0.01f); + double hPa; + double referencePressure; }; diff --git a/competition/src/competition/filter/WiFi.h b/competition/src/competition/filter/WiFi.h index a3c142d..581e8f8 100644 --- a/competition/src/competition/filter/WiFi.h +++ b/competition/src/competition/filter/WiFi.h @@ -134,13 +134,13 @@ public: // older entry? higher sigma! // high signal strength? higher sigma! - const Timestamp age = curTS - ap.getTimestamp(); + const Timestamp age = curTS - ap.getTimestamp(); - const float sigma = 11.0 + 1.2 * 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 1e7a370..6960b30 100755 --- a/competition/src/competition/main.cpp +++ b/competition/src/competition/main.cpp @@ -1,10 +1,12 @@ #include "FileReader.h" +#include #include "filter/Structs.h" #include "Plotti.h" #include "Structs.h" +#include #include "filter/WiFi.h" #include "filter/Logic.h" @@ -24,12 +26,22 @@ #include #include #include +#include +#include +#include +#include +#include #include "Optimizer.h" #include "Scaler.h" #include +#include #include +#include +#include +#include +#include #include #include @@ -76,12 +88,12 @@ void testScaler() { #include //frank -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/"; +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 { @@ -91,6 +103,7 @@ struct DataSetup { int minWifiOccurences; VAPGrouper::Mode vapMode; Scaler scaler; + int buildingNum; }; /** all configured datasets */ @@ -104,20 +117,24 @@ struct Data { dataDir + "car/logfile_CAR_R1_S3.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" + dataDir + "car/logfile_CAR_R2_S4.txt", + dataDir + "eval/evaluation_logfile_06.txt", // s3 + dataDir + "eval/evaluation_logfile_09.txt" // s4 }, dataDir + "car/wifiParams.txt", 40, VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, - Scaler (1282, 818, 40.31320308, -3.48367648, -8.77680000, 0.05207600) + Scaler (1282, 818, 40.31320308, -3.48367648, -8.77680000, 0.05207600), + + 10 }; DataSetup UAH = { - mapDir + "UAH/UAH9.xml", + mapDir + "UAH/UAH13_rawObstacles.xml", { dataDir + "uah/logfile_UAH_R1_S3.txt", @@ -125,37 +142,47 @@ struct Data { dataDir + "uah/logfile_UAH_R2_S3.txt", dataDir + "uah/logfile_UAH_R2_S4.txt", dataDir + "uah/logfile_UAH_R4_S3.txt", - dataDir + "uah/logfile_UAH_R4_S4.txt" + dataDir + "uah/logfile_UAH_R4_S4.txt", + dataDir + "eval/evaluation_logfile_03.txt", + dataDir + "eval/evaluation_logfile_05.txt", + dataDir + "eval/evaluation_logfile_07.txt", + dataDir + "eval/evaluation_logfile_08.txt" }, dataDir + "uah/wifiParams.txt", 40, VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, - Scaler (1869, 1869, 40.51312440, -3.34959080, -40.73112000, 0.07596002) + Scaler (1869, 1869, 40.51312440, -3.34959080, -40.73112000, 0.07596002), + + 20 }; DataSetup UJI_TI = { - mapDir + "UJI-TI/UJI-TI4.xml", + mapDir + "UJI-TI/UJI-TI4.xml", { dataDir + "uji-ti/logfile_UJITI_R1_NEXUS5.txt", dataDir + "uji-ti/logfile_UJITI_R2_NEXUS5.txt", + dataDir + "eval/evaluation_logfile_01.txt", + dataDir + "eval/evaluation_logfile_04.txt" }, dataDir + "uji-ti/wifiParams.txt", 15, VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, - Scaler (748, 1046, 39.99322125, -0.06862410, 29.57638723, 0.08556493) + Scaler (748, 1046, 39.99322125, -0.06862410, 29.57638723, 0.08556493), + + 40 }; DataSetup UJI_UB = { - mapDir + "UJI-UB/UJI-UB4.xml", + mapDir + "UJI-UB/UJI-UB5.xml", { dataDir + "uji-ub/logfile_UJIUB_R1n_S3.txt", @@ -163,6 +190,7 @@ struct Data { dataDir + "uji-ub/logfile_UJIUB_R2n_S3.txt", dataDir + "uji-ub/logfile_UJIUB_R2r_S3.txt", dataDir + "uji-ub/logfile_UJIUB_R3_S3.txt", + dataDir + "eval/evaluation_logfile_02.txt" }, dataDir + "uji-ub/wifiParams.txt", @@ -170,7 +198,9 @@ struct Data { VAPGrouper::Mode::DISABLED, // BEWARE! not all images have the same size! - Scaler (4362, 1959, 39.99380328, -0.07389222, 29.54974856, 0.01700000) + Scaler (4362, 1959, 39.99380328, -0.07389222, 29.54974856, 0.01700000), + + 30 }; @@ -243,253 +273,487 @@ void optimize(Floorplan::IndoorMap* map, Scaler& scaler, VAPGrouper::Mode vapMod } - - Floorplan::IndoorMap* MyState::map; -int main(int argc, char** argv) { +void run(DataSetup setup, int numFile, std::string folder) { - testScaler(); + //testScaler(); - // the dataset to use - DataSetup setup = data.UAH; + // the dataset to use + //DataSetup setup = data.UAH; - // load the floorplan - Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); - MyState::map = map; + // 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 (and save) wifi parameters + //optimize(map, setup.scaler, setup.vapMode, setup.minWifiOccurences, setup.training, setup.wifiParams); - // testing - map->floors[0]->obstacles.clear(); + // testing + //map->floors[0]->obstacles.clear(); - // fetch previously optimized wifi paramters + // fetch previously optimized wifi paramters - WiFiModelX model(setup.vapMode); - model.read(setup.wifiParams, map); + WiFiModelX model(setup.vapMode); + model.read(setup.wifiParams, map); - // build the grid - Grid grid(20); - GridFactory factory(grid); - factory.build(map); - Importance::addImportance(grid); +// // build the grid + Grid grid(20); +// GridFactory factory(grid); +// factory.build(map); + +// std::filebuf fb; +// fb.open (dataDir + "uah/grid13",std::ios::out); +// std::ostream os(&fb); +// grid.write(os); +// fb.close(); + + + std::filebuf fb; + if (fb.open (dataDir + "uah/grid13",std::ios::in)) + { + std::istream is(&fb); + grid.read(is); + } + fb.close(); + + + Importance::addImportance(grid); + + + FileReader fr(setup.training[numFile]); + + + //partikel gehen nicht schnell genug nach oben oder unten + //und wlan zwischen oben/unten ist nicht unterschiedlich genug? + + + std::vector> scans = fr.getWiFiGroupedByTime(); + Interpolator path = fr.getPath(); + + Plotti plot; + plot.addFloors(map); + plot.addOutline(map); + plot.addStairs(map); + plot.gp << "set autoscale xy\n"; + //plot.addGrid(grid); + + for (FileReader::TS pos : fr.positions) { + Point3 p3 = setup.scaler.convert3D(pos.data.lat, pos.data.lon, pos.data.floorNr); + + /** check if long/lat calculation is correct just to be sure :)*/ + IPIN pos2 = setup.scaler.toIPIN3(p3); + + Assert::isNear(pos.data.lat, pos2.lat, 0.000000001, "lat not equal"); + Assert::isNear(pos.data.lon, pos2.lon, 0.000000001, "lon not equal"); + Assert::isNear(pos.data.floorNr, static_cast(pos2.floorNr), 0.000000001f, "floorNr not equal"); + + plot.addGroundTruthNode(p3); + } + MyControl ctrl; ctrl.numSteps = 0; + MyObs obs; + + int numParticles = 8000; + int numBSParticles = 500; + + PFEval* eval = new PFEval(setup.scaler, model, grid); + + //std::unique_ptr init = + K::ParticleFilterHistory pf(numParticles, std::unique_ptr(new PFInit(grid))); + pf.setTransition(std::unique_ptr(new PFTrans(grid, &ctrl))); + pf.setEvaluation(std::unique_ptr(eval)); + + pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingSimple())); + //pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingPercent(0.04))); + pf.setNEffThreshold(0.5); + + //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.95))); + //pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationKernelDensity())); - FileReader fr(setup.training[1]); + /** Toni Smoothing Stuff */ + //create the backward smoothing filter + K::BackwardSimulation bf(numBSParticles); + //bf = new K::CondensationBackwardFilter; + bf.setSampler( std::unique_ptr>(new K::CumulativeSampler())); + + bool smoothing_resample = false; + //bf->setNEffThreshold(1.0); + + //Smoothing using Simple Trans + //bf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); + //bf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationRegionalWeightedAverage())); + bf.setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationOrderedWeightedAverage(0.95f))); + + if(smoothing_resample) + bf.setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); + bf.setTransition(std::unique_ptr( new BFTrans(grid)) ); + + //History of all estimated particles. Used for smoothing + std::vector>> pfHistory; + std::vector smoothedEst; + std::vector tsHistory; + + std::vector pathEst; - //partikel gehen nicht schnell genug nach oben oder unten - //und wlan zwischen oben/unten ist nicht unterschiedlich genug? + float lastUpdateMS = 0; - - std::vector> scans = fr.getWiFiGroupedByTime(); - Interpolator path = fr.getPath(); - - Plotti plot; - plot.addFloors(map); - plot.addOutline(map); - plot.addStairs(map); - //plot.addGrid(grid); - - for (FileReader::TS pos : fr.positions) { - Point3 p3 = setup.scaler.convert3D(pos.data.lat, pos.data.lon, pos.data.floorNr); - plot.addGroundTruthNode(p3); - } - - - - MyControl ctrl; ctrl.numSteps = 0; - MyObs obs; - - int numParticles = 4000; - - PFEval* eval = new PFEval(setup.scaler, model); - - //std::unique_ptr init = - K::ParticleFilter pf(numParticles, std::unique_ptr(new PFInit(grid))); - pf.setTransition(std::unique_ptr(new PFTrans(grid, &ctrl))); - pf.setEvaluation(std::unique_ptr(eval)); - - pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingSimple())); - //pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingPercent(0.04))); - pf.setNEffThreshold(1.0); - - //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.95))); - //pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationKernelDensity())); - - - - - float lastUpdateMS = 0; - - //StepDetector sd; - //TurnDetector td; - StepDetection sd; - TurnDetection td; - RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); - PressureTendence baroTend(Timestamp::fromSec(5)); + //StepDetector sd; + //TurnDetector td; + StepDetection sd; + TurnDetection td; + RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); + PressureTendence baroTend(Timestamp::fromSec(5)); ActivityButterPressure act; + ActivityButterPressurePercent actProbs; - K::Statistics errorStats; + K::Statistics errorStats; + K::Statistics errorStatsSmoothing; - struct TurnTest { - float rad = 0; - Point3 pos = Point3(81, 49, 0); - } tt; + struct TurnTest { + float rad = 0; + Point3 pos = Point3(81, 49, 0); + } tt; + + //file writing for offline competition + long int t = static_cast(time(NULL)); + + std::ofstream compTrack3; + compTrack3.open ("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/ipin_" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv"); + compTrack3.setf(std::ios::fixed,std::ios::floatfield); + //compTrack3.precision(8); + + std::ofstream compTrack4; + compTrack4.open ("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/ipinSmoothing_" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv"); + compTrack4.setf(std::ios::fixed,std::ios::floatfield); + + std::ofstream errorFile; + errorFile.open ("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/error_" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv"); + + std::ofstream errorFileSmoothing; + errorFileSmoothing.open ("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/errorSmoothing_" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv"); - // parse each sensor-value within the offline data - for (const FileReader::Entry& e : fr.getEntries()) { + MovingAVG hPaRef(20); + MovingAVG hPaCur(5); + bool newReferencePressureIsSet = false; + float pressureResetDelay = 0; - const float ms = e.ts * 1000; - const Timestamp ts = Timestamp::fromMS(ms); + //Just for IPIN Competition - if (e.type == FileReader::Sensor::WIFI) { - obs.wifis = fr.getWiFiGroupedByTime()[e.idx].data; + // parse each sensor-value within the offline data + for (const FileReader::Entry& e : fr.getEntries()) { - } else if (e.type == FileReader::Sensor::ACC) { - if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) { - ++ctrl.numSteps; - tt.pos.x += std::cos(tt.rad) * 0.7; - tt.pos.y += std::sin(tt.rad) * 0.7; - } - const FileReader::TS& _acc = fr.getAccelerometer()[e.idx]; - td.addAccelerometer(Timestamp::fromSec(ts), _acc.data); + const float ms = e.ts * 1000; + const Timestamp ts = Timestamp::fromMS(ms); - } else if (e.type == FileReader::Sensor::GYRO) { - const FileReader::TS& _gyr = fr.getGyroscope()[e.idx]; - //td.update(ctrl.turnAngle, e.ts * 1000, fr.getGyroscope()[e.idx].data); - const float delta = td.addGyroscope(Timestamp::fromSec(ts), _gyr.data); - ctrl.turnAngle += delta; - tt.rad += delta; + if (e.type == FileReader::Sensor::WIFI) { + obs.wifis = fr.getWiFiGroupedByTime()[e.idx].data; - } else if (e.type == FileReader::Sensor::GPS) { - obs.gps = fr.getGPS()[e.idx].data; + } else if (e.type == FileReader::Sensor::ACC) { + if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) { + ++ctrl.numSteps; + tt.pos.x += std::cos(tt.rad) * 0.7; + tt.pos.y += std::sin(tt.rad) * 0.7; + } + const FileReader::TS& _acc = fr.getAccelerometer()[e.idx]; + td.addAccelerometer(Timestamp::fromSec(ts), _acc.data); - } else if (e.type == FileReader::Sensor::BARO) { - relBaro.add(ts, fr.getBarometer()[e.idx].data); - //baroTend.add(ts, fr.getBarometer()[e.idx].data); - //std::cout << "add tendence" << std::endl; + } else if (e.type == FileReader::Sensor::GYRO) { + const FileReader::TS& _gyr = fr.getGyroscope()[e.idx]; + //td.update(ctrl.turnAngle, e.ts * 1000, fr.getGyroscope()[e.idx].data); + const float delta = td.addGyroscope(Timestamp::fromSec(ts), _gyr.data); + ctrl.turnAngle += delta; + tt.rad += delta; + + } else if (e.type == FileReader::Sensor::GPS) { + obs.gps = fr.getGPS()[e.idx].data; + + } else if (e.type == FileReader::Sensor::BARO) { + 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(); /** toni activity */ - ctrl.barometer.act = act.add(ts, fr.getBarometer()[e.idx].data); + std::pair actAndBaro; + actAndBaro = act.add(ts, fr.getBarometer()[e.idx].data); + ctrl.barometer.act = actAndBaro.first; + obs.act = actAndBaro.first; - } + ctrl.barometer.actProbs = actProbs.add(ts, fr.getBarometer()[e.idx].data); + obs.actProbs = ctrl.barometer.actProbs; - if (ms - lastUpdateMS > 500) { + //NOTE: the pressure value of actAndBaro was smoothed using a butterworth filter. + hPaCur.add(actAndBaro.second); + obs.hPa = hPaCur.get(); - obs.curTS = e.ts; + + //do we need to reset the relative pressure? reset it, + //everytime we recognize a floor change. add a blocking window afterwards to prevent false detections + //NOTE: This is updated all 100 ms. + if(obs.act != ActivityButterPressure::Activity::STAY && ms - pressureResetDelay < 20000){ + + //call the reset just once. + if(!newReferencePressureIsSet){ + obs.referencePressure = hPaRef.get(); + newReferencePressureIsSet = true; + } + ctrl.barometer.resetRelPressure = false; + + //TODO: ms instead of fixed number 10000 ms (10sec) should be good + }else if(ms - pressureResetDelay < 20000 && newReferencePressureIsSet){ + //do nothing + }else{ + newReferencePressureIsSet = false; + hPaRef.add(actAndBaro.second); + obs.referencePressure = hPaRef.get(); + ctrl.barometer.resetRelPressure = true; + pressureResetDelay = ms; + } + + + /** baro for eval test */ +// hPaHist.push_back(fr.getBarometer()[e.idx].data.hPa); + +// float diff = 0.0f; +// if(hPaHist.size() > 22){ +// const float front = (hPaHist.end()[-3] + hPaHist.end()[-2] + hPaHist.end()[-1]) / 3; +// const float back = (hPaHist.end()[-19] + hPaHist.end()[-20] + hPaHist.end()[-21]) / 3;; + +// diff = front - back; +// } + + } + + if (ms - lastUpdateMS > 500) { + + obs.curTS = e.ts; // ((PFTrans*)pf.getTransition())->modWifi.obs = &obs; + Point3 mapPos = Point3(0,0,0); + if(!fr.getPositions().empty()){ + FileReader::Position worldPos = path.get(e.ts); + //Point2 mapPos2 = setup.scaler.convert(worldPos.lat, worldPos.lon); + //Point3 mapPos = Point3(mapPos2.x, mapPos2.y, 0); + mapPos = setup.scaler.convert3D(worldPos.lat, worldPos.lon, worldPos.floorNr); + } - FileReader::Position worldPos = path.get(e.ts); - //Point2 mapPos2 = setup.scaler.convert(worldPos.lat, worldPos.lon); - //Point3 mapPos = Point3(mapPos2.x, mapPos2.y, 0); - const Point3 mapPos = setup.scaler.convert3D(worldPos.lat, worldPos.lon, worldPos.floorNr); + MyState est = pf.update(&ctrl, obs); + Point3 estPos = est.position.inMeter(); + + //save the current estimation for later smoothing. + pfHistory.push_back(pf.getNonResamplingParticles()); + tsHistory.push_back(ms); + pathEst.push_back(estPos); + + //discrete floors + //estPos.z = static_cast(estPos.z + 0.5); + + //write into file in IPIN format + IPIN estIPIN = setup.scaler.toIPIN3(estPos); + + //ts in sec, lat, long, floorID, buildingID + //we need to interpolate the results to exact 0.5 timestamps + compTrack3 << std::setprecision(1) << (std::llround(ts.sec()*2) / 2.0) << "," << std::setprecision(9) << estIPIN.lon << "," << estIPIN.lat << "," << static_cast(estIPIN.floorNr + 0.5) << "," << setup.buildingNum << "\n"; + + plot.pInterest.clear(); + + /** check if ground truth is correctly printed + static int floor = 0; + if(floor < map->floors.size() + 1){ + plot.printSingleFloor("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/", floor); + ++floor; + }else{ + plot.gp << "set terminal qt\n"; + plot.gp << "set zrange [0 :" << floor << "]\n"; + } + + */ - MyState est = pf.update(&ctrl, obs); - Point3 estPos = est.position.inMeter(); + static float angleSum = 0; angleSum += ctrl.turnAngle; + //plot.showAngle(1, ctrl.turnAngle); + plot.showAngle(2, angleSum + M_PI); + + //plot.debugWiFi(eval->model, obs.wifis, obs.curTS); + //plot.debugProb(grid, std::bind(&PFEval::getGPS, eval, std::placeholders::_1, std::placeholders::_2), obs); + //plot.debugProb(grid, std::bind(&PFEval::getWIFI, eval, std::placeholders::_1, std::placeholders::_2), obs); + //plot.debugProb(grid, std::bind(&PFEval::getALL, eval, std::placeholders::_1, std::placeholders::_2), obs); + + plot.setEst(estPos); + plot.setGT(mapPos); + plot.addEstimationNode(estPos); + //plot.addParticles(pf.getParticles()); + //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\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"; + int minutes = static_cast(ts.sec()) / 60; + plot.gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << static_cast(static_cast(ts.sec())%60) << "'\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); + errorStats.add(err_m); + + // debug + //std::cout << err_m << std::endl; + //std::cout << errorStats.asString() << std::endl; + //std::cout << "ACHSENERKENNUNG AUF ACC FUER ROTATION??" << std::endl; + + errorFile << err_m << "\n"; + + plot.show(); + usleep(100*100); - plot.pInterest.clear(); + lastUpdateMS = ms; - static float angleSum = 0; angleSum += ctrl.turnAngle; - //plot.showAngle(1, ctrl.turnAngle); - plot.showAngle(2, angleSum + M_PI); + // reset control + ctrl.numSteps = 0; + ctrl.turnAngle = 0; + } - //plot.debugWiFi(eval->model, obs.wifis, obs.curTS); - //plot.debugProb(grid, std::bind(&PFEval::getGPS, eval, std::placeholders::_1, std::placeholders::_2), obs); - //plot.debugProb(grid, std::bind(&PFEval::getWIFI, eval, std::placeholders::_1, std::placeholders::_2), obs); - //plot.debugProb(grid, std::bind(&PFEval::getALL, eval, std::placeholders::_1, std::placeholders::_2), obs); + } - plot.setEst(estPos); - plot.setGT(mapPos); - plot.addParticles(pf.getParticles()); - plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\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.98, 0.98 'act:" << ctrl.barometer.act << "'\n"; - - // error between GT and estimation - float err_m = mapPos.getDistance(estPos); - errorStats.add(err_m); - - // debug - std::cout << err_m << std::endl; - std::cout << errorStats.asString() << std::endl; - std::cout << "ACHSENERKENNUNG AUF ACC FUER ROTATION??" << std::endl; - - plot.show(); - usleep(1000*10); - - lastUpdateMS = ms; - - // reset control - ctrl.numSteps = 0; - ctrl.turnAngle = 0; - } + errorFile.close(); + // Smoothing Part starts here + // ========================= // + bool smoothing = false; + //File + //std::ofstream statsout2("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/smoothed_" + runName + ".stats"); + //stats.reset(); + if(smoothing){ + bf.reset(); + + MyState estBF; + + //iterate thru all particle sets from T to t (currentParticleSetToSmoothAtTimeT) + for(long i = pfHistory.size() - 1; i >= 0 ; i -= 1){ + estBF = bf.update(pfHistory[i]); + smoothedEst.push_back(estBF.position.inMeter()); + } + + std::reverse(smoothedEst.begin(), smoothedEst.end()); + int itSize = bf.getbackwardParticles().size() - 1; + + for(unsigned int t = 0; t < smoothedEst.size(); ++t){ + + const Point3 curSmoothedEst = smoothedEst[t]; + + Point3 mapPos = Point3(0,0,0); + if(!fr.getPositions().empty()){ + FileReader::Position worldPos = path.get(tsHistory[t] / 1000.0); + mapPos = setup.scaler.convert3D(worldPos.lat, worldPos.lon, worldPos.floorNr); + } + + // error between GT and estimation + float err_m = mapPos.getDistance(curSmoothedEst); + errorStatsSmoothing.add(err_m); + + IPIN estIPINSmoothed = setup.scaler.toIPIN3(curSmoothedEst); + + //ts in sec, lat, long, floorID, buildingID + //we need to interpolate the results to exact 0.5 timestamps + compTrack4 << std::setprecision(1) << (std::llround((tsHistory[t] / 1000.0) *2) / 2.0) << "," << std::setprecision(9) << estIPINSmoothed.lon << "," << estIPINSmoothed.lat << "," << static_cast(estIPINSmoothed.floorNr + 0.5) << "," << setup.buildingNum << "\n"; - } + // plot + plot.setEst(curSmoothedEst); + plot.setGT(mapPos); + plot.addEstimationNodeSmoothed(curSmoothedEst); + plot.addParticles(bf.getbackwardParticles()[itSize - t]); + //plot.show(); + errorFileSmoothing << err_m << "\n"; - /* - K::Statistics stats; + // prevent gnuplot errors + //usleep(100*50); + } + } - for (FileReader::TS scan : scans) { + errorFileSmoothing.close(); + compTrack3.close(); + compTrack4.close(); - //scan.data.groupVAPs(); + //Write the current plotti buffer into file + std::ofstream plotFile; + plotFile.open("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/gnu" + std::to_string(numFile) + "_" + std::to_string(t) + ".gp"); + plot.saveToFile(plotFile); + plotFile.close(); - plot.pColorPoints.clear(); + for(int i = 0; i < map->floors.size(); ++i){ + plot.printSingleFloor("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), i); + plot.show(); + usleep(1000*10); + } - // ground truth - plot.pInterest.clear(); - FileReader::Position worldPos = path.get(scan.ts); - Point2 mapPos2 = scaler.convert(worldPos.lat, worldPos.lon); - Point3 mapPos(mapPos2.x, mapPos2.y, 0); + plot.printSideView("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 90); + plot.show(); - plot.pInterest.add(K::GnuplotPoint3(mapPos.x, mapPos.y, mapPos.z)); + plot.printSideView("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 0); + plot.show(); - // update - stats.add( model.getProbability(scan, mapPos) ); - - const float step = 1.0; - float z = 0; - for (float x = -20; x < 90; x += step) { - for (float y = -10; y < 60; y += step) { - - double prob = model.getProbability(scan, Point3(x,y,z)); - plot.pColorPoints.add(K::GnuplotPoint3(x,y,z), prob); - - } - } - - plot.show(); - usleep(1000*50); - - } - - std::cout << stats.asString() << std::endl; -*/ - - - sleep(1000); + plot.printOverview("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t)); + plot.show(); + sleep(1); + +} + +int main(int argc, char** argv) { + + for(int i = 0; i < 11; ++i){ + + + + //Testing files + //run(data.UAH, 0, "UAHMAP"); + //run(data.UAH, 1, "UAHMAP"); + //run(data.UAH, 2, "UAHMAP"); + //run(data.UAH, 3, "UAHMAP"); + //run(data.UAH, 4, "UAHMAP"); + //run(data.UAH, 5, "UAHMAP"); + +// run(data.UJI_UB, 0, "UJIBU"); +// run(data.UJI_UB, 1, "UJIBU"); +// run(data.UJI_UB, 2, "UJIBU"); +// run(data.UJI_UB, 3, "UJIBU"); +// run(data.UJI_UB, 4, "UJIBU"); + +// run(data.UJI_TI, 0, "UJITI"); +// run(data.UJI_TI, 1, "UJITI"); + +// run(data.CAR, 0, "CAR"); +// run(data.CAR, 1, "CAR"); +// run(data.CAR, 2, "CAR"); +// run(data.CAR, 3, "CAR"); + + //Evaluation files +// run(data.UJI_TI, 2, "EVAL"); // 1 no Baro +// run(data.UJI_UB, 5, "EVAL"); // 2 + run(data.UAH, 6, "EVAL"); // 3 +// run(data.UJI_TI, 3, "EVAL"); // 4 no Baro +// run(data.UAH, 7, "EVAL"); // 5 +// run(data.CAR, 4, "EVAL"); // 6 +// run(data.UAH, 8, "EVAL"); // 7 +// run(data.UAH, 9, "EVAL"); // 8 +// run(data.CAR, 5, "EVAL2"); // 9 + + + } } diff --git a/competition/src/competition/map/CAR/CAR9.xml b/competition/src/competition/map/CAR/CAR9.xml new file mode 100644 index 0000000..082c325 --- /dev/null +++ b/competition/src/competition/map/CAR/CAR9.xml @@ -0,0 +1,284 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/competition/src/competition/map/UAH/UAH10_rawObstacles.xml b/competition/src/competition/map/UAH/UAH10_rawObstacles.xml new file mode 100644 index 0000000..51208f6 --- /dev/null +++ b/competition/src/competition/map/UAH/UAH10_rawObstacles.xml @@ -0,0 +1,1361 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/competition/src/competition/map/UAH/UAH11_rawObstacles.xml b/competition/src/competition/map/UAH/UAH11_rawObstacles.xml new file mode 100644 index 0000000..1c8b259 --- /dev/null +++ b/competition/src/competition/map/UAH/UAH11_rawObstacles.xml @@ -0,0 +1,1196 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/competition/src/competition/map/UAH/UAH12_rawObstacles.xml b/competition/src/competition/map/UAH/UAH12_rawObstacles.xml new file mode 100644 index 0000000..e76f9b9 --- /dev/null +++ b/competition/src/competition/map/UAH/UAH12_rawObstacles.xml @@ -0,0 +1,1165 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/competition/src/competition/map/UAH/UAH13_rawObstacles.xml b/competition/src/competition/map/UAH/UAH13_rawObstacles.xml new file mode 100644 index 0000000..f95787f --- /dev/null +++ b/competition/src/competition/map/UAH/UAH13_rawObstacles.xml @@ -0,0 +1,1096 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/competition/src/competition/map/UAH/UAH9.xml b/competition/src/competition/map/UAH/UAH9.xml new file mode 100644 index 0000000..c89a1b9 --- /dev/null +++ b/competition/src/competition/map/UAH/UAH9.xml @@ -0,0 +1,512 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/competition/src/competition/map/UJI-TI/UJI-TI4.xml b/competition/src/competition/map/UJI-TI/UJI-TI4.xml new file mode 100644 index 0000000..43ebdd3 --- /dev/null +++ b/competition/src/competition/map/UJI-TI/UJI-TI4.xml @@ -0,0 +1,198 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/competition/src/competition/map/UJI-UB/UJI-UB4.xml b/competition/src/competition/map/UJI-UB/UJI-UB4.xml new file mode 100644 index 0000000..a348659 --- /dev/null +++ b/competition/src/competition/map/UJI-UB/UJI-UB4.xml @@ -0,0 +1,227 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/competition/src/competition/map/UJI-UB/UJI-UB5.xml b/competition/src/competition/map/UJI-UB/UJI-UB5.xml new file mode 100644 index 0000000..032e704 --- /dev/null +++ b/competition/src/competition/map/UJI-UB/UJI-UB5.xml @@ -0,0 +1,245 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tex/bare_conf.dvi b/tex/bare_conf.dvi index 31b2c92..2f5cb1e 100644 Binary files a/tex/bare_conf.dvi and b/tex/bare_conf.dvi differ diff --git a/tex/bare_conf.tex b/tex/bare_conf.tex index 3aa133f..e5704df 100644 --- a/tex/bare_conf.tex +++ b/tex/bare_conf.tex @@ -129,6 +129,9 @@ \input{misc/keywords} \input{misc/functions} +\IEEEoverridecommandlockouts +\IEEEpubid{\makebox[\columnwidth]{\hfill 978-1-5090-2425-4/16/\$31.00~\copyright~2016 IEEE} +\hspace{\columnsep}\makebox[\columnwidth]{ }} \begin{document} %