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:
@@ -64,7 +64,7 @@ ADD_DEFINITIONS(
|
||||
-fstack-protector-all
|
||||
|
||||
-g
|
||||
-O1
|
||||
-O2
|
||||
-DWITH_TESTS
|
||||
-DWITH_ASSERTIONS
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
@@ -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)));
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -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();
|
||||
|
||||
|
||||
@@ -1,4 +1,11 @@
|
||||
#ifndef ORIENTATIONOBSERVATION_H
|
||||
#define ORIENTATIONOBSERVATION_H
|
||||
|
||||
/** android device orientation */
|
||||
struct OrientationObservation {
|
||||
|
||||
float values[3];
|
||||
|
||||
};
|
||||
|
||||
#endif // ORIENTATIONOBSERVATION_H
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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 |
@@ -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;
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user