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

added motion axis stuff

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

252
FileReader.h
View File

@@ -1,252 +0,0 @@
#ifndef FILEREADER_H
#define FILEREADER_H
#include <fstream>
#include <Indoor/Exception.h>
#include <vector>
#include <Indoor/math/Interpolator.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/sensors/pressure/BarometerData.h>
#include <Indoor/sensors/imu/AccelerometerData.h>
#include <Indoor/sensors/imu/GyroscopeData.h>
#include <Indoor/sensors/beacon/BeaconMeasurements.h>
#include <unordered_map>
#include <Indoor/geo/Point2.h>
#include <Indoor/grid/factory/v2/GridFactory.h>
#include <Indoor/grid/factory/v2/Importance.h>
#include <Indoor/floorplan/v2/Floorplan.h>
class FileReader {
public:
template <typename T> struct TS {
const uint64_t ts;
T data;
TS(const uint64_t ts) : ts(ts) {;}
TS(const uint64_t ts, const T& data) : ts(ts), data(data) {;}
};
enum class Sensor {
ACC,
GYRO,
WIFI,
POS,
BARO,
BEACON,
};
/** entry for one sensor */
struct Entry {
Sensor type;
uint64_t ts;
int idx;
Entry(Sensor type, uint64_t ts, int idx) : type(type), ts(ts), idx(idx) {;}
};
std::vector<TS<int>> groundTruth;
std::vector<TS<WiFiMeasurements>> wifi;
std::vector<TS<BeaconMeasurement>> beacon;
std::vector<TS<AccelerometerData>> acc;
std::vector<TS<GyroscopeData>> gyro;
std::vector<TS<BarometerData>> barometer;
/** ALL entries */
std::vector<Entry> entries;
public:
FileReader(const std::string& file) {
parse(file);
}
const std::vector<Entry>& getEntries() const {return entries;}
const std::vector<TS<int>>& getGroundTruth() const {return groundTruth;}
const std::vector<TS<WiFiMeasurements>>& getWiFiGroupedByTime() const {return wifi;}
const std::vector<TS<BeaconMeasurement>>& getBeacons() const {return beacon;}
const std::vector<TS<AccelerometerData>>& getAccelerometer() const {return acc;}
const std::vector<TS<GyroscopeData>>& getGyroscope() const {return gyro;}
const std::vector<TS<BarometerData>>& getBarometer() const {return barometer;}
private:
void parse(const std::string& file) {
std::ifstream inp(file);
if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);}
while(!inp.eof() && !inp.bad()) {
uint64_t ts;
char delim;
int idx = -1;
std::string data;
inp >> ts;
inp >> delim;
inp >> idx;
inp >> delim;
inp >> data;
if (idx == 8) {parseWiFi(ts, data);}
else if (idx == 9) {parseBeacons(ts, data);}
else if (idx == 99) {parseGroundTruth(ts, data);}
else if (idx == 0) {parseAccelerometer(ts, data);}
else if (idx == 3) {parseGyroscope(ts, data);}
else if (idx == 5) {parseBarometer(ts, data);}
// TODO: this is a hack...
// the loop is called one additional time after the last entry
// and keeps the entries of entry
}
inp.close();
}
void parseAccelerometer(const uint64_t ts, const std::string& data) {
const int pos1 = data.find(';');
const int pos2 = data.find(';', pos1+1);
const std::string x = data.substr(0, pos1);
const std::string y = data.substr(pos1+1, pos2-pos1-1);
const std::string z = data.substr(pos2+1);
TS<AccelerometerData> elem(ts, AccelerometerData(std::stof(x), std::stof(y), std::stof(z)));
acc.push_back(elem);
entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1));
}
void parseGyroscope(const uint64_t ts, const std::string& data) {
const int pos1 = data.find(';');
const int pos2 = data.find(';', pos1+1);
const std::string x = data.substr(0, pos1);
const std::string y = data.substr(pos1+1, pos2-pos1-1);
const std::string z = data.substr(pos2+1);
TS<GyroscopeData> elem(ts, GyroscopeData(std::stof(x), std::stof(y), std::stof(z)));
gyro.push_back(elem);
entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1));
}
void parseWiFi(const uint64_t ts, const std::string& data) {
std::string tmp = data;
// add new wifi reading
wifi.push_back(TS<WiFiMeasurements>(ts, WiFiMeasurements()));
entries.push_back(Entry(Sensor::WIFI, ts, wifi.size()-1));
// process all APs
while(!tmp.empty()) {
std::string mac = tmp.substr(0, 17);
tmp = tmp.substr(17);
assert(tmp[0] == ';'); tmp = tmp.substr(1);
std::string freq = tmp.substr(0, 4);
tmp = tmp.substr(4);
assert(tmp[0] == ';'); tmp = tmp.substr(1);
int pos = tmp.find(';');
std::string rssi = tmp.substr(0, pos);
tmp = tmp.substr(pos);
assert(tmp[0] == ';'); tmp = tmp.substr(1);
// append AP to current scan-entry
WiFiMeasurement e(AccessPoint(mac), std::stoi(rssi), Timestamp::fromMS(ts));
wifi.back().data.entries.push_back(e);
}
}
void parseBeacons(const uint64_t ts, const std::string& data) {
std::string tmp = data;
std::string mac = tmp.substr(0, 17);
tmp = tmp.substr(17);
assert(tmp[0] == ';'); tmp = tmp.substr(1);
std::string rssi = tmp;
//yes the timestamp is redundant here, but in case of multiusage...
TS<BeaconMeasurement> e(ts, BeaconMeasurement(Timestamp::fromMS(ts), Beacon(mac), std::stoi(rssi)));
beacon.push_back(e);
entries.push_back(Entry(Sensor::BEACON, ts, beacon.size()-1));
}
void parseGroundTruth(const uint64_t ts, const std::string& data) {
const int pos1 = data.find(';');
std::string gtIndex = data.substr(0, pos1);
TS<int> elem(ts, std::stoi(gtIndex));
groundTruth.push_back(elem);
}
void parseBarometer(const uint64_t ts, const std::string& data) {
const int pos1 = data.find(';');
const std::string hPa = data.substr(0, pos1);
TS<BarometerData> elem(ts, BarometerData(std::stof(hPa)));
barometer.push_back(elem);
entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1));
}
const Interpolator<uint64_t, Point2> getGroundTruthPath(Floorplan::IndoorMap& map, std::vector<int> gtPath) const {
// finde alle positionen der waypoints im gtPath aus map
std::unordered_map<int, Point2> waypointsMap;
for(Floorplan::Floor* f : map.floors){
for (Floorplan::POI* poi : f->pois){
waypointsMap.insert({std::stoi(poi->name), poi->pos});
}
}
// bringe aufgenommene gt punkte der app in unordered_map
std::unordered_map<int, const uint64_t> waypointsApp;
for(TS<int> val : groundTruth){
waypointsApp.insert({val.data, val.ts});
}
// bringe diese in richtige reihenfolge und füge timestamp hinzu
Interpolator<uint64_t, Point2> interpol;
for(int id : gtPath){
auto itMap = waypointsMap.find(id);
if(itMap == waypointsMap.end()) {throw "not found";}
auto itApp = waypointsApp.find(id);
if(itApp == waypointsApp.end()) {throw "not found";}
interpol.add(itApp->second, itMap->second);
}
return interpol;
}
};
#endif // FILEREADER_H

16
Settings.h
View File

@@ -10,9 +10,9 @@ namespace Settings {
const int numParticles = 5000; const int numParticles = 5000;
namespace IMU { namespace IMU {
const float turnSigma = 1.5; // 3.5 const float turnSigma = 2.5; // 3.5
const float stepLength = 0.80; const float stepLength = 1.00;
const float stepSigma = 0.40; //toni changed const float stepSigma = 0.15; //toni changed
} }
const float smartphoneAboveGround = 1.3; const float smartphoneAboveGround = 1.3;
@@ -32,9 +32,9 @@ namespace Settings {
} }
namespace WiFiModel { namespace WiFiModel {
constexpr float sigma = 13.0; constexpr float sigma = 8.0;
/** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */ /** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */
constexpr float TXP = -48; constexpr float TXP = -46;
constexpr float EXP = 2.5; constexpr float EXP = 2.5;
constexpr float WAF = -5.0; constexpr float WAF = -5.0;
@@ -47,10 +47,10 @@ namespace Settings {
} }
namespace BeaconModel { namespace BeaconModel {
constexpr float sigma = 13.0; constexpr float sigma = 8.0;
constexpr float TXP = -48; constexpr float TXP = -71;
constexpr float EXP = 1.5; constexpr float EXP = 1.5;
constexpr float WAF = -8.0; //-5 constexpr float WAF = -20.0; //-5 //20??
} }
namespace MapView3D { namespace MapView3D {

49
filter/Logic.h
View File

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

7
filter/Structs.h
View File

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

115
main.cpp
View File

@@ -1,4 +1,3 @@
#include "FileReader.h"
#include <iostream> #include <iostream>
#include "filter/Structs.h" #include "filter/Structs.h"
@@ -15,12 +14,13 @@
#include <Indoor/grid/factory/v2/Importance.h> #include <Indoor/grid/factory/v2/Importance.h>
#include <Indoor/geo/Point2.h> #include <Indoor/geo/Point2.h>
#include <Indoor/sensors/offline/FileReader.h>
#include <KLib/math/statistics/Statistics.h> #include <KLib/math/statistics/Statistics.h>
#include <Indoor/sensors/imu/TurnDetection.h> #include <Indoor/sensors/imu/TurnDetection.h>
#include <Indoor/sensors/imu/StepDetection.h> #include <Indoor/sensors/imu/StepDetection.h>
#include <Indoor/sensors/imu/MotionDetection.h>
#include <Indoor/sensors/pressure/RelativePressure.h> #include <Indoor/sensors/pressure/RelativePressure.h>
#include <Indoor/sensors/radio/WiFiGridEstimator.h> #include <Indoor/sensors/radio/WiFiGridEstimator.h>
#include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h> #include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
@@ -62,13 +62,13 @@ struct DataSetup {
std::string wifiParams; std::string wifiParams;
int minWifiOccurences; int minWifiOccurences;
VAPGrouper::Mode vapMode; VAPGrouper::Mode vapMode;
int buildingNum; std::string grid;
}; };
/** all configured datasets */ /** all configured datasets */
struct Data { struct Data {
DataSetup SHL = { DataSetup BERKWERK = {
mapDir + "SHL/SHL25.xml", mapDir + "SHL/SHL25.xml",
@@ -86,7 +86,27 @@ struct Data {
dataDir + "bergwerk/wifiParams.txt", dataDir + "bergwerk/wifiParams.txt",
40, 40,
VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO,
10 mapDir + "SHL/grid25.dat"
};
DataSetup IPIN2015 = {
mapDir + "SHL/SHL_IPIN2015_gt.xml",
{
dataDir + "ipin2015/galaxy/Path0/1433581471902.csv",
dataDir + "ipin2015/nexus/Path0/1433606195078.csv",
dataDir + "ipin2015/galaxy/Path1/1433587749492.csv",
dataDir + "ipin2015/nexus/Path1/1433606670723.csv",
dataDir + "ipin2015/galaxy/Path2/1433581471902.csv",
dataDir + "ipin2015/nexus/Path2/1433607251262.csv",
dataDir + "eiszeit/path2/1479986737368.csv"
},
dataDir + "bergwerk/wifiParams.txt",
40,
VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO,
mapDir + "SHL/grid_IPIN2015_gt.dat"
}; };
} data; } data;
@@ -105,22 +125,25 @@ void run(DataSetup setup, int numFile, std::string folder) {
Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml"); Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml");
BeaconModelLogDistCeiling beaconModel(map); BeaconModelLogDistCeiling beaconModel(map);
BeaconModelLogDistCeiling::APEntry beacon1(Point3(69.84f,45.26f,3.8f+3.4f+1.2f),-81,Settings::BeaconModel::EXP, Settings::BeaconModel::WAF); beaconModel.loadBeaconsFromMap(map, Settings::BeaconModel::TXP, Settings::BeaconModel::EXP, Settings::BeaconModel::WAF);
beaconModel.addBeacon(MACAddress("48:EF:8D:77:66:DF"), beacon1); Assert::isFalse(beaconModel.getAllBeacons().empty(), "no Beacons stored within the map.xml");
BeaconModelLogDistCeiling::APEntry beacon2(Point3(69.84f,45.26f,3.8f+3.4f+1.2f),-81,Settings::BeaconModel::EXP, Settings::BeaconModel::WAF);
beaconModel.addBeacon(MACAddress("6F:5F:39:0C:51:E4"), beacon2);
BeaconModelLogDistCeiling::APEntry beacon3(Point3(69.84f,45.26f,3.8f+3.4f+1.2f),-81,Settings::BeaconModel::EXP, Settings::BeaconModel::WAF);
beaconModel.addBeacon(MACAddress("49:23:D8:7F:E8:D2"), beacon3);
//beaconModel.loadBeaconsFromMap(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
Assert::isFalse(beaconModel.getAllBeacons().empty(), "no AccessPoints stored within the map.xml");
// build the grid // build the grid
std::ifstream inp(setup.grid, std::ifstream::binary);
Grid<MyNode> grid(20); Grid<MyNode> grid(20);
GridFactory<MyNode> factory(grid);
factory.build(map); // grid.dat empty? -> build one and save it
if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
std::ofstream onp;
onp.open(setup.grid);
GridFactory<MyNode> factory(grid);
factory.build(map);
grid.write(onp);
} else {
grid.read(inp);
}
// add node-importance // add node-importance
Importance::addImportance(grid); Importance::addImportance(grid);
@@ -128,14 +151,16 @@ void run(DataSetup setup, int numFile, std::string folder) {
// stamp WiFi signal-strengths onto the grid // stamp WiFi signal-strengths onto the grid
WiFiGridEstimator::estimate(grid, WiFiModel, Settings::smartphoneAboveGround); WiFiGridEstimator::estimate(grid, WiFiModel, Settings::smartphoneAboveGround);
// reading file // reading file
FileReader fr(setup.training[numFile]); FileReader fr(setup.training[numFile]);
//std::vector<int> gt;
// doing ground truth stuff // doing ground truth stuff
//fr.getGroundTruthPath(map, gt_1); std::vector<int> path_0 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
std::vector<int> path_1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
std::vector<int> path_2 = {29, 28, 27, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2, 19, 18, 17, 16, 15, 14, 13, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29};
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, path_1);
Plotti plot; Plotti plot;
@@ -151,20 +176,21 @@ void run(DataSetup setup, int numFile, std::string folder) {
ctrl.resetAfterTransition(); ctrl.resetAfterTransition();
MyObs obs; MyObs obs;
int numParticles = 5000; int numParticles = 10000;
PFEval* eval = new PFEval(WiFiModel, beaconModel, grid); PFEval* eval = new PFEval(WiFiModel, beaconModel, grid);
//filter init //filter init
//std::unique_ptr<PFInit> init = //std::unique_ptr<PFInit> init =
K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(numParticles, std::unique_ptr<PFInit>(new PFInit(grid))); //K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(numParticles, std::unique_ptr<PFInit>(new PFInit(grid)));
K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(numParticles, std::unique_ptr<PFInitFixed>(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 1080.0f), 90.0f)));
pf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl))); pf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
pf.setEvaluation(std::unique_ptr<PFEval>(eval)); pf.setEvaluation(std::unique_ptr<PFEval>(eval));
//resampling //resampling
//pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>())); pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.04))); //pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.4)));
pf.setNEffThreshold(0.85); pf.setNEffThreshold(0.95);
//estimation //estimation
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>())); //pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -177,11 +203,13 @@ void run(DataSetup setup, int numFile, std::string folder) {
StepDetection sd; StepDetection sd;
TurnDetection td; TurnDetection td;
MotionDetection md;
RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
K::Statistics<float> errorStats; K::Statistics<float> errorStats;
//file writing for offline competition //file writing for error data
long int t = static_cast<long int>(time(NULL)); long int t = static_cast<long int>(time(NULL));
std::ofstream errorFile; std::ofstream errorFile;
errorFile.open (errorDir + folder + "/error_" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv"); errorFile.open (errorDir + folder + "/error_" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv");
@@ -209,22 +237,34 @@ void run(DataSetup setup, int numFile, std::string folder) {
} else if (e.type == FileReader::Sensor::GYRO) { } else if (e.type == FileReader::Sensor::GYRO) {
const FileReader::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx]; const FileReader::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
const float delta = td.addGyroscope(ts, _gyr.data); const float delta_gyro = td.addGyroscope(ts, _gyr.data);
ctrl.turnSinceLastTransition_rad += delta;
ctrl.turnSinceLastTransition_rad += delta_gyro;
} else if (e.type == FileReader::Sensor::BARO) { } else if (e.type == FileReader::Sensor::BARO) {
relBaro.add(ts, fr.getBarometer()[e.idx].data); relBaro.add(ts, fr.getBarometer()[e.idx].data);
obs.relativePressure = relBaro.getPressureRealtiveToStart(); obs.relativePressure = relBaro.getPressureRealtiveToStart();
obs.sigmaPressure = relBaro.getSigma(); obs.sigmaPressure = relBaro.getSigma();
} else if (e.type == FileReader::Sensor::LIN_ACC) {
md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
} else if (e.type == FileReader::Sensor::GRAVITY) {
md.addGravity(ts, fr.getGravity()[e.idx].data);
Eigen::Vector2f curVec = md.getCurrentMotionAxis();
ctrl.motionDeltaAngle_rad = md.getMotionChangeInRad();
} }
if (ts.ms() - lastTimestamp.ms() > 500) { if (ts.ms() - lastTimestamp.ms() > 500) {
obs.currentTime = ts; obs.currentTime = ts;
MyState est = pf.update(&ctrl, obs); MyState est = pf.update(&ctrl, obs);
Point3 estPos = est.position.inMeter(); Point3 estPos = est.position.inMeter();
//current ground truth position
Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms()));
plot.pInterest.clear(); plot.pInterest.clear();
// plotting stuff // plotting stuff
@@ -238,7 +278,7 @@ void run(DataSetup setup, int numFile, std::string folder) {
//plot.debugProb(grid, std::bind(&PFEval::getALL, 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.setEst(estPos);
//plot.setGT(mapPos); plot.setGT(gtPos);
plot.addEstimationNode(estPos); plot.addEstimationNode(estPos);
plot.addParticles(pf.getParticles()); 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 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";
@@ -249,12 +289,12 @@ void run(DataSetup setup, int numFile, std::string folder) {
//plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" << ctrl.barometer.act << "'\n"; //plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" << ctrl.barometer.act << "'\n";
// error between GT and estimation // error between GT and estimation
//float err_m = mapPos.getDistance(estPos); float err_m = gtPos.getDistance(estPos);
// errorStats.add(err_m); errorStats.add(err_m);
//errorFile << err_m << "\n"; errorFile << err_m << "\n";
plot.show(); plot.show();
usleep(100*10); usleep(33*10);
lastTimestamp = ts; lastTimestamp = ts;
@@ -267,6 +307,9 @@ void run(DataSetup setup, int numFile, std::string folder) {
errorFile.close(); errorFile.close();
std::cout << "Statistical Analysis: " << std::endl;
std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << std::endl;
//Write the current plotti buffer into file //Write the current plotti buffer into file
std::ofstream plotFile; std::ofstream plotFile;
plotFile.open(errorDir + std::to_string(numFile) + "_" + std::to_string(t) + ".gp"); plotFile.open(errorDir + std::to_string(numFile) + "_" + std::to_string(t) + ".gp");
@@ -295,7 +338,7 @@ void run(DataSetup setup, int numFile, std::string folder) {
int main(int argc, char** argv) { int main(int argc, char** argv) {
//Testing files //Testing files
run(data.SHL, 6, "EVAL"); // Nexus vor //run(data.BERKWERK, 6, "EVALBERGWERK"); // Nexus vor
run(data.IPIN2015, 3, "EVALIPIN2015"); // Nexus Path2
} }