changed visualisation

added new eval code for new walkers
improved barometer (moving avg and median)
floorplan-staircase-fixes
disabled step-turn (now part of transition)
added abs-orientation-reader (for testing)
added beacons
This commit is contained in:
2016-02-05 20:21:46 +01:00
parent 9e9b6882cd
commit 716b004f3c
15 changed files with 188 additions and 179 deletions

View File

@@ -64,7 +64,7 @@ ADD_DEFINITIONS(
-fstack-protector-all
-g
-O1
-O2
-DWITH_TESTS
-DWITH_ASSERTIONS

View File

@@ -26,8 +26,9 @@ public:
K::GnuplotSplotElementLines gridEdges;
K::GnuplotSplotElementPoints particles;
K::GnuplotSplotElementLines particleDir;
K::GnuplotSplotElementLines groundTruth;
K::GnuplotSplotElementLines estPath;
K::GnuplotSplotElementLines groundTruth;
public:
@@ -70,6 +71,8 @@ public:
for (const Line2& l : f.getObstacles()) {
const K::GnuplotPoint3 p1(l.p1.x, l.p1.y, height.cm());
const K::GnuplotPoint3 p2(l.p2.x, l.p2.y, height.cm());
//if (l.p1.x > 3000) {continue;}
//if (l.p1.y > 3000) {continue;}
floors.addSegment(p1, p2);
}

View File

@@ -7,7 +7,9 @@
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
#include <Indoor/grid/walk/GridWalkPushForward.h>
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
#include <Indoor/grid/walk/GridWalkSimpleControl.h>
#include <Indoor/grid/walk/GridWalkPathControl.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
@@ -25,7 +27,7 @@ public:
pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
std::vector<int> wp = path1; std::reverse(wp.begin(), wp.end());
std::vector<int> wp = path1;// std::reverse(wp.begin(), wp.end());
MyGridNode& start = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[wp.front()]) );
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[wp.back()]) );
@@ -35,21 +37,22 @@ public:
//GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<MyGridNode>();
//GridWalkLightAtTheEndOfTheTunnel<MyGridNode>* walk = new GridWalkLightAtTheEndOfTheTunnel<MyGridNode>(grid, DijkstraMapper(grid), end);
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
//GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
// path1
//sr = new SensorReader("./measurements/path1/1/1454345312844.csv"); // forward
//srt = new SensorReaderTurn("./measurements/path1/1/Turns.txt");
//srs = new SensorReaderStep("./measurements/path1/1/Steps2.txt");
sr = new SensorReader("./measurements/path1/2/1454345421125.csv"); // backward
srt = new SensorReaderTurn("./measurements/path1/2/Turns.txt");
srs = new SensorReaderStep("./measurements/path1/2/Steps2.txt");
sr = new SensorReader("./measurements/path1/1/1454345312844.csv"); // forward
srt = new SensorReaderTurn("./measurements/path1/1/Turns.txt");
srs = new SensorReaderStep("./measurements/path1/1/Steps2.txt");
// sr = new SensorReader("./measurements/path1/2/1454345421125.csv"); // backward
// srt = new SensorReaderTurn("./measurements/path1/2/Turns.txt");
// srs = new SensorReaderStep("./measurements/path1/2/Steps2.txt");
// path2
//sr = new SensorReader("./measurements/path2/1/1454345775306.csv"); // forward
//srt = new SensorReaderTurn("./measurements/path2/1/Turns.txt");
//srs = new SensorReaderStep("./measurements/path2/1/Steps2.txt");
// sr = new SensorReader("./measurements/path2/1/1454345775306.csv"); // forward
// srt = new SensorReaderTurn("./measurements/path2/1/Turns.txt");
// srs = new SensorReaderStep("./measurements/path2/1/Steps2.txt");
//sr = new SensorReader("./measurements/path2/2/1454346071347.csv"); // backward
//srt = new SensorReaderTurn("./measurements/path2/2/Turns.txt");
//srs = new SensorReaderStep("./measurements/path2/2/Steps2.txt");
@@ -62,11 +65,22 @@ public:
// srt = new SensorReaderTurn("./measurements/path3/2/Turns.txt");
// srs = new SensorReaderStep("./measurements/path3/2/Steps2.txt");
// path4
// sr = new SensorReader("./measurements/path4/1454595382218.csv"); // forward
// srt = new SensorReaderTurn("./measurements/path4/Turns.txt");
// srs = new SensorReaderStep("./measurements/path4/Steps2.txt");
gtw = getGroundTruthWay(*sr, floors.gtwp, wp);
}
void setEvalFails() {
}
//wifi also uniform dist 0/1 fuer bereiche die OK sind?
//steps hochzaehlen weil mehr als einer in einer transition??
//increase regional average region
@@ -87,9 +101,9 @@ public:
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.10)) );
// state estimation step
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.50f)));
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.50f)));
}

View File

@@ -27,6 +27,7 @@
#include "../frank/WiFiSensorReader.h"
#include "../frank/BeaconSensorReader.h"
#include "../frank/OrientationSensorReader.h"
class EvalBase {
@@ -57,6 +58,7 @@ protected:
std::vector<int> path1 = {29, 28,27,26,255,25,24,23,22,21,20};
std::vector<int> path2 = {19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6};
std::vector<int> path3 = {5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
std::vector<int> path4 = {29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44};
public:
@@ -71,6 +73,12 @@ public:
vis.addFloor(floors.f2, floors.h2);
vis.addFloor(floors.f3, floors.h3);
vis.floors.setColorHex("#666666");
vis.groundTruth.setCustomAttr("dashtype 3");
vis.groundTruth.setColorHex("#009900");
vis.gp << "unset cbrange\n";
}
static GridPoint conv(const Point3& p) {
@@ -110,7 +118,7 @@ public:
void run() {
// sensor numbers
const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5;
const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; const int s_orientation = 6;
const int s_linearAcceleration = 2;
std::list<TurnObservation> turn_observations;
@@ -198,8 +206,13 @@ public:
}
case s_linearAcceleration:{
baroSensorReader.readVerticalAcceleration(se);
break;
baroSensorReader.readVerticalAcceleration(se);
break;
}
case s_orientation: {
obs.orientation = OrientationSensorReader::read(se);
break;
}
}
@@ -235,7 +248,7 @@ public:
const Point3 curEst = est.pCur;
// error calculation. compare ground-truth to estimation
const int offset = 0; // 750
const int offset = 750;
const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
const Point3 diff = curEst - curGT;
@@ -258,11 +271,17 @@ public:
vis.addEstPath(pathEst);
vis.setEstAndShould(curEst, curGT);
if (obs.barometer != nullptr) {
vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n";
}
vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
//vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";
Point2 p1(0.1, 0.1);
Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.1;
vis.gp << "set arrow 111 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";
Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.05;
//Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05;
vis.gp << "set arrow 999 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";
vis.show();

View File

@@ -1,4 +1,11 @@
#ifndef ORIENTATIONOBSERVATION_H
#define ORIENTATIONOBSERVATION_H
/** android device orientation */
struct OrientationObservation {
float values[3];
};
#endif // ORIENTATIONOBSERVATION_H

View File

@@ -1,4 +1,42 @@
#ifndef ORIENTATIONSENSORREADER_H
#define ORIENTATIONSENSORREADER_H
#include "../reader/SensorReader.h"
#include "OrientationObservation.h"
#include <cassert>
class OrientationSensorReader {
public:
/** get wifi observation data from one CSV entry */
static OrientationObservation read(const SensorEntry& se) {
std::string tmp = se.data;
OrientationObservation obs;
size_t pos1 = tmp.find(';');
size_t pos2 = tmp.find(';', pos1+1);
size_t pos3 = tmp.find(';', pos2+1);
assert(pos1 != std::string::npos);
assert(pos2 != std::string::npos);
assert(pos3 != std::string::npos);
const std::string s1 = tmp.substr(0, pos1);
const std::string s2 = tmp.substr(pos1+1, pos2-pos1-1);
const std::string s3 = tmp.substr(pos2+1, pos3-pos2-1);
obs.values[0] = std::stof(s1);
obs.values[1] = std::stof(s2);
obs.values[2] = std::stof(s3);
return obs;
}
};
#endif // ORIENTATIONSENSORREADER_H

View File

@@ -22,6 +22,7 @@ public:
const double pl = 2.7; // 2.7
const double tx = -46;
const float ibPLE = 1.9;
addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300, Helper::getHeight(3), tx, pl);
addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930, Helper::getHeight(3), tx, pl);
@@ -52,7 +53,9 @@ public:
addAP(("00:04:96:6B:46:09"), "I.0.xx", 6860, 3690, Helper::getHeight(0), tx, pl);
addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, Helper::getHeight(0), tx, pl); // vague!!
addBeacon("48:EF:8D:77:66:DF", -81, ibPLE, 6984, 4526, Helper::getHeight(2));
addBeacon("6F:5F:39:0C:51:E4", -81, ibPLE, 7829, 3916, 200);
addBeacon("49:23:D8:7F:E8:D2", -81, ibPLE, 6946, 4536, Helper::getHeight(1));
// OLD

View File

@@ -15,15 +15,16 @@ class WiFiAP {
public:
/** the AP's MAC-Address */
MACAddress mac;
const MACAddress mac;
/** the AP's readable SSID */
std::string ssid;
const std::string ssid;
double tx;
/** AP tx-power */
const float tx;
/** path loss for this ap. for testing */
double pl;
const float pl;

View File

@@ -21,48 +21,27 @@ public:
return 1;
const float mdlWalked_m = state.walkState.distanceWalked_m;
// see: particle-filter-control-data
// const float mdlWalked_m = state.walkState.distanceWalked_m;
((MyState&)state).walkState.distanceWalked_m = 0;
// ((MyState&)state).walkState.distanceWalked_m = 0;
const float stepSize_m = 0.71;
const float sensSigma_m = 0.05 + (0.05 * obs->steps);
const float sensWalked_m = obs->steps * stepSize_m;
// const float stepSize_m = 0.71;
// const float sensSigma_m = 0.05 + (0.05 * obs->steps);
// const float sensWalked_m = obs->steps * stepSize_m;
if (obs->steps > 1) {
int i = 0;
int j = i+1; ++j;
}
const double prob = K::NormalDistribution::getProbability(sensWalked_m, sensSigma_m, mdlWalked_m);
if (prob != prob) {
throw 1;
}
return prob;
// float a = 1.0;
// float mu_distance = 0;
// float sigma_distance = 0;
// if(obs->step) {
// a = 1.0;
// mu_distance = mu_walk;
// sigma_distance = sigma_walk;
// if (obs->steps > 1) {
// int i = 0;
// int j = i+1; ++j;
// }
// else {
// a = 0.0;
// mu_distance = mu_stop;
// sigma_distance = sigma_stop;
// const double prob = K::NormalDistribution::getProbability(sensWalked_m, sensSigma_m, mdlWalked_m);
// if (prob != prob) {
// throw 1;
// }
// //Mixed Gaussian model: 1st Gaussian = step, 2nd Gaussian = no step
// const double p = a * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance) +
// (1.0-a) * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance);
// return p;
// return prob;
}

View File

@@ -19,49 +19,22 @@ public:
return 1;
// get the current heading-change
const float delta = Angle::radToDeg(state.walkState.headingChange_rad);
// see: particle-filter-control-data
// // get the current heading-change
// const float delta = Angle::radToDeg(state.walkState.headingChange_rad);
// and reset it as it was evaluated
((MyState&) state).walkState.headingChange_rad = 0;
// // and reset it as it was evaluated
// ((MyState&) state).walkState.headingChange_rad = 0;
// proability?
const float sigma = 15.0;
const double prob = K::NormalDistribution::getProbability(obs->delta_heading, sigma, delta);
// // proability?
// const float sigma = 15.0;
// const double prob = K::NormalDistribution::getProbability(obs->delta_heading, sigma, delta);
if (prob != prob) {
throw 1;
}
// if (prob != prob) {
// throw 1;
// }
return prob;
// //Particle's heading change
// double delta_heading_particle = state.heading - state.heading_old;
// //Correct offset of the heading change
// if (delta_heading_particle < -180) {
// delta_heading_particle += 360;
// }
// else if (delta_heading_particle > 180) {
// delta_heading_particle -= 360;
// }
// //Switch between simple and improved evaluation
// //"Simple" only evaluates the deviation between the measured heading and the particle heading change using
// //normal distribution
// //if(simple) {
// double sigma_delta_heading = sigma_heading;
// const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle);
// return p;
// // }
// return prob;
}

View File

@@ -73,8 +73,6 @@ void testModelWalk() {
vis.show();
}
sleep(1000);
}
@@ -85,12 +83,12 @@ int main(void) {
// testModelWalk();
// Eval1 eval;
// eval.setEval1();
// eval.run();
Eval1 eval;
eval.setEval1();
eval.run();
PaperVisImportance::createImportance();
PaperVisImportance::createPath();
// PaperVisImportance::createImportance();
// PaperVisImportance::createPath();
return 0;

View File

@@ -3,6 +3,8 @@
#include "../frank/WiFiObservation.h"
#include "../frank/BeaconObservation.h"
#include "../frank/OrientationObservation.h"
#include "../toni/BarometerObservation.h"
#include "../lukas/StepObservation.h"
#include "../lukas/TurnObservation.h"
@@ -15,6 +17,8 @@ struct MyObservation {
/** wifi observation */
WiFiObservation wifi;
OrientationObservation orientation;
/** barometer observation data (if any) */
BarometerObservation* barometer = nullptr;

View File

@@ -42,17 +42,17 @@
guidetolerance="10"
inkscape:pageopacity="0"
inkscape:pageshadow="2"
inkscape:window-width="3200"
inkscape:window-height="1671"
inkscape:window-width="1600"
inkscape:window-height="822"
id="namedview3234"
showgrid="true"
inkscape:zoom="1.6989779"
inkscape:cx="881.41659"
inkscape:cy="1487.0248"
inkscape:zoom="2.4027176"
inkscape:cx="2636.6172"
inkscape:cy="1428.0719"
inkscape:window-x="0"
inkscape:window-y="55"
inkscape:window-y="0"
inkscape:window-maximized="1"
inkscape:current-layer="layer26"
inkscape:current-layer="layer12"
inkscape:object-nodes="true"
units="px"
showborder="true"
@@ -2511,7 +2511,7 @@
inkscape:groupmode="layer"
id="layer12"
inkscape:label="floor_1"
style="display:none">
style="display:inline">
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 49.606299,1853.1496 605.905511,0"
@@ -3188,12 +3188,22 @@
d="m 442.91339,712.20469 63.77952,0"
id="path5836"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 2653.937,389.76374 0,24.80315"
id="path4999"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 2777.9528,414.56689 -124.0158,0"
id="path5001"
inkscape:connector-curvature="0" />
</g>
<g
inkscape:groupmode="layer"
id="layer9"
inkscape:label="floor_2"
style="display:inline"
style="display:none"
transform="translate(0,4.2364502e-5)">
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
@@ -3927,6 +3937,11 @@
d="m 442.91339,712.20465 63.77952,0"
id="path5834"
inkscape:connector-curvature="0" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 2462.5984,389.7637 0,70.86615"
id="path4996"
inkscape:connector-curvature="0" />
</g>
<g
inkscape:groupmode="layer"
@@ -4722,10 +4737,10 @@
inkscape:groupmode="layer"
id="layer17"
inkscape:label="staircase_0_1"
style="display:none">
style="display:inline">
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 2664.5669,655.51178 102.7559,0 0,-155.90552"
d="m 2664.5669,655.51178 102.7559,0 0,-265.74804"
id="path4988"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccc" />

Before

Width:  |  Height:  |  Size: 238 KiB

After

Width:  |  Height:  |  Size: 238 KiB

View File

@@ -13,7 +13,6 @@ public:
double getProbability(const MyState& state, const BarometerObservation* obs) const {
//return 1;
// //rho_z
double barometerSigma = 0.12+0.04;//0.09;

View File

@@ -6,6 +6,7 @@
#include <sstream>
#include <Indoor/math/MovingAVG.h>
#include <Indoor/math/MovingMedian.h>
//circular_buffer<double> measurementHistory(1000);
@@ -13,21 +14,19 @@
class BarometerSensorReader{
private:
// circular_buffer<double> measurementHistory;
MovingAVG<float> avg;
// NOTE: median or avg?
MovingMedian<float> avg;
MovingMedian<float> avgStart;
// avg: lower size, median: bigger still fine
static constexpr int avgSize = 10;
static constexpr int startAvgSize = 10;
public:
BarometerSensorReader(): avg(3) {
// if(!USE_STATIC_CIRCULAR_BUFFERING){
// //8.33min
// measurementHistory.reserve(10000);
// }
// else{
// //30 * 500ms = 1,5s
// measurementHistory.reserve(30);
// }
BarometerSensorReader(): avg(avgSize), avgStart(startAvgSize) {
;
}
BarometerObservation* readBarometer(const SensorEntry& se) {
@@ -36,62 +35,19 @@ public:
BarometerObservation* obs = new BarometerObservation();
// get the next hPa reading and average it
avg.add(stod(tmp));
const double hPa = avg.get();
avg.add(std::stof(tmp));
// everything realtive to the first measurement
static double first_hPa = 0;
if (avg.getNumUsed() < avg.getSize()) {first_hPa = avg.get();}
// average the first few readings as reference
if (avgStart.getNumUsed() < startAvgSize) {
avgStart.add(std::stof(tmp));
}
obs->hpa = hPa - first_hPa;
//std::cout << obs->hpa << std::endl;
// current average relative to the start-average
obs->hpa = avg.get() - avgStart.get();
// done
return obs;
// if(USE_BAROMETER_SMOOTHING_RC_LOWPASS){
// //smoothing with alpha value
// if(measurementHistory.size() > 1){
// double alpha = 0.1;
// double lastMeasurement = measurementHistory[measurementHistory.size() - 1];
// currentMeasurement = (alpha * currentMeasurement) + ((1.0 - alpha) * lastMeasurement);
// obs->hpa = currentMeasurement;
// }else{
// obs->hpa = 0;
// }
// measurementHistory.push_back(currentMeasurement);
// }
// else if (USE_BAROMETER_SMOOTHING_HEAD_TAIL){
// currentMeasurement = hPa;
// measurementHistory.push_back(currentMeasurement);
// // calculate the relative air pressure by getting the mean of the first and last three entrys of the history
// // and subtract them.
// if (measurementHistory.size() > 5){
// double meanTail = (measurementHistory[0] + measurementHistory[1] + measurementHistory[2]) / 3.0;
// double meanHead = (measurementHistory[measurementHistory.size() - 1] + measurementHistory[measurementHistory.size() - 2] + measurementHistory[measurementHistory.size() - 3]) / 3.0;
// obs->hpa = meanHead - meanTail;
// }
// else{
// obs->hpa = 0;
// }
// }
// else //no data smoothing
// {
// measurementHistory.push_back(currentMeasurement);
// obs->hpa = currentMeasurement;
// }
// return obs;
}
//TODO