added activity detection

code/eval/DebugShortestPath.h

@@ -33,9 +33,9 @@ public:
 
 	}
 
-	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
+	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad, Activity act) {
 
-		GridWalkState<T> s = GridWalkShortestPathControl<T>::getDestination(grid, start, distance_m, headChange_rad);
+		GridWalkState<T> s = GridWalkShortestPathControl<T>::getDestination(grid, start, distance_m, headChange_rad, act);
 
 		if (this->recalc == 0){
 			vis.estPath.clear();

code/eval/FixedLagEvalBase.h

@@ -27,9 +27,11 @@
 #include "../reader/SensorReader.h"
 #include "../reader/SensorReaderStep.h"
 #include "../reader/SensorReaderTurn.h"
+#include "../reader/SensorReaderAccel.h"
 
 #include "../lukas/TurnObservation.h"
 #include "../lukas/StepObservation.h"
+#include "../lukas/ActivityDetection.h"
 
 #include "../toni/BarometerSensorReader.h"
 
@@ -133,6 +135,8 @@ public:
 
         // sensor numbers
         const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; const int s_orientation = 6;
+		const int s_accel = 0;
+
         //const int s_linearAcceleration = 2;
 
         std::list<TurnObservation> turn_observations;
@@ -141,6 +145,9 @@ public:
         //Create an BarometerSensorReader
         BarometerSensorReader baroSensorReader;
 
+		// activity detection
+		ActivityDetection actDet;
+
 
         //Read all turn Observations
         while(srt->hasNext()) {
@@ -234,9 +241,17 @@ public:
 
                 case s_barometer: {
                     obs.barometer = baroSensorReader.readBarometer(se);
+					actDet.addBaro(baroSensorReader.getHPA(se));
                     break;
                 }
 
+				case s_accel: {
+					float acc[3];
+					SensorReaderAccel sre; sre.read(se, acc);
+					actDet.addAccel(acc);
+					break;
+				}
+
 //					case s_linearAcceleration:{
 //						baroSensorReader.readVerticalAcceleration(se);
 //						break;
@@ -267,6 +282,11 @@ public:
 
             }
 
+			// currently detected activity
+			// TODO: feed sensor values!
+			ctrl.currentActivitiy = actDet.getCurrentActivity();
+
+
 
             // time for a transition?
             if (se.ts - lastTransitionTS > MiscSettings::timeSteps) {
@@ -365,6 +385,7 @@ public:
                         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 112 '" << "Act: " << actDet.toString() << "' at screen 0.1,0.15\n";
 
                     //double avgAngleRad = estBF.avgAngle * 180/3.14159265359;
 

code/eval/PaperVisImportance.h

@@ -178,7 +178,7 @@ public:
 			if (i % 250 == 0) {std::cout << i << std::endl;}
 			const MyGridNode& nStart = gnEnd;
 			GridWalkState<MyGridNode> sStart(&nStart, Heading::rnd());
-			GridWalkState<MyGridNode> sEnd = walk.getDestination(grid, sStart, 135, 0);
+			GridWalkState<MyGridNode> sEnd = walk.getDestination(grid, sStart, 135, 0, Activity::UNKNOWN);
 			(void) sEnd;
 
 		}

code/eval/SmoothingEvalBase.h

@@ -394,9 +394,10 @@ public:
         oSError.close();
 
         // plot-data
-        std::ofstream oPath("/tmp/path_" + runName + ".dat");	vis.groundTruth.addDataTo(oPath); oPath.close();
+		std::ofstream oPath("/tmp/path_" + runName + ".dat");		vis.groundTruth.addDataTo(oPath); oPath.close();		// ground truth
-        std::ofstream oEst("/tmp/est_" + runName + ".dat");		vis.estPath.addDataTo(oEst); oEst.close();
+		std::ofstream oEstN("/tmp/est_norm" + runName + ".dat");	vis.estPath.addDataTo(oEstN); oEstN.close();			// estimation via filter itself
-        std::ofstream oFloor("/tmp/floors.dat");				vis.floors.addDataTo(oFloor); oFloor.close();
+		std::ofstream oEstS("/tmp/est_smooth" + runName + ".dat");	vis.smoothPath.addDataTo(oEstS); oEstS.close();			// estimation via smoothing
+		std::ofstream oFloor("/tmp/floors.dat");					vis.floors.addDataTo(oFloor); oFloor.close();
 
         std::ofstream oPlot("/tmp/plot_" + runName + ".gp");
 
@@ -416,9 +417,10 @@ public:
         oPlot << "unset ztics\n";
 
         oPlot << "splot \\\n";
-        oPlot << "'floors.dat'              skip 21 notitle with lines lc rgb '#777777', \\\n";
+		oPlot << "'floors.dat'						skip 21 notitle with lines lc rgb '#777777', \\\n";
-        oPlot << "'path_fixed_interval.dat'	skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n";
+		oPlot << "'path_fixed_interval.dat'			skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n";
-        oPlot << "'est_fixed_interval.dat'	skip 21 notitle with lines lw 2.5 lc rgb '#000099' ";
+		oPlot << "'est_norm_fixed_interval.dat'		skip 21 notitle with lines lw 2.5 lc rgb '#000099', ";
+		oPlot << "'est_smooth_fixed_interval.dat'	skip 21 notitle with lines lw 2.5 lc rgb '#000000' ";
 
         oPlot.close();
     }

code/lukas/Activities.h

@@ -0,0 +1,6 @@
+#ifndef ACTIVITIES_H
+#define ACTIVITIES_H
+
+
+
+#endif // ACTIVITIES_H

code/lukas/ActivityDetection.h

@@ -0,0 +1,121 @@
+#ifndef ACTIVITYDETECTION_H
+#define ACTIVITYDETECTION_H
+
+#include <string>
+#include "Activities.h"
+#include <vector>
+
+#include <Indoor/math/MovingAVG.h>
+#include <Indoor/math/MovingMedian.h>
+
+#include <KLib/math/statistics/Statistics.h>
+
+
+#include <Indoor/math/Distributions.h>
+
+/**
+ * classification of pedestrian activities using sensor inputs.
+ * sensor inputs are passed into this element
+ * recognized acitivities are the output
+ *
+ */
+class ActivityDetection {
+
+private:
+
+	/** the currently detected activity */
+	Activity current = Activity::UNKNOWN;
+
+	K::Statistics<float> mag;
+	K::Statistics<float> hpa;
+
+	MovingMedian<float> hpaAvg;
+
+public:
+
+	ActivityDetection() : hpaAvg(3) {;}
+
+	/** add accelerometer values */
+	void addAccel(float accel[3]) {
+
+		float magnitude = std::sqrt(accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2]);
+		mag.add(magnitude - 9.81f);
+
+	}
+
+	/** add barometer values */
+	void addBaro(float hpa) {
+		hpaAvg.add(hpa);
+		float smoothed = hpaAvg.get();
+		this->hpa.add(smoothed);
+		if (this->hpa.getCount() > 50) {analyze();}
+	}
+
+	struct ActClass {
+		Activity act;
+		Distribution::Normal<float> barometer;
+		Distribution::Normal<float> magnitude;
+		ActClass(const Activity act, float muMag, float muBaro, float varMag, float varBaro) :
+			act(act), barometer(muBaro, std::sqrt(varBaro)), magnitude(muMag, std::sqrt(varMag)) {
+			;
+		}
+		float getProbability(const float hpaRange, const float varMag) const {
+			return	barometer.getProbability(hpaRange) *
+					magnitude.getProbability(varMag);
+		}
+	};
+
+	void analyze() {
+
+//		std::vector<ActClass> classes = {
+//			ActClass(Activity::STANDING, 0.042118, 0.01, 0.002322, 0.001),
+//			ActClass(Activity::STAIRS, 3.561667, 0.035, 1.392631, 0.002),		// stair up
+//			ActClass(Activity::STAIRS, 5.529414, -0.035, 3.416315, 0.002),		// stair down
+//			ActClass(Activity::WALKING, 1.669506, 0.01, 0.273880, 0.001),
+//		};
+
+//		auto comp = [&] (const ActClass& a, const ActClass& b) {
+//			return a.getProbability(hpa.getRange(), mag.getStdDev()) < b.getProbability(hpa.getRange(), mag.getStdDev());
+//		};
+
+//		auto it = std::max_element(classes.begin(), classes.end(), comp);
+
+//		current = (*it).act;
+
+
+		if (mag.getStdDev() < 0.3) {
+			current = Activity::STANDING;
+		} else {
+			if (hpa.getRange() > 0.035) {
+				current = Activity::STAIRS;
+			} else {
+				current = Activity::WALKING;
+			}
+		}
+
+//		current = (Activity) idx;
+
+		mag.reset();
+		hpa.reset();
+
+	}
+
+	/** get the currently detected activity */
+	Activity getCurrentActivity() const {
+		return current;
+	}
+
+	std::string toString() const {
+		switch (current) {
+			case Activity::UNKNOWN:		return "unknown";
+			case Activity::STANDING:	return "standing";
+			case Activity::WALKING:		return "walking";
+			case Activity::STAIRS:		return "stairs";
+			case Activity::ELEVATOR:	return "elevator";
+		}
+		throw "should not happen";
+	}
+
+};
+
+#endif // ACTIVITYDETECTION_H

code/particles/MyControl.h

@@ -1,12 +1,17 @@
 #ifndef MYCONTROL_H
 #define MYCONTROL_H
 
+#include "../lukas/Activities.h"
+
+/** current control data for the transition step */
 struct MyControl {
 
 	float walked_m = 0;
 
 	float headingChange_rad = 0;
 
+	Activity currentActivitiy = Activity::UNKNOWN;
+
 };
 
 #endif // MYCONTROL_H

code/particles/MyEvaluation.h

@@ -71,16 +71,18 @@ public:
 				weight *= beaconEval.getProbability(p.state, observation);
 			}
 
-			if (useStep) {
+			// CONTROL!
-				weight *= stepEval.getProbability(p.state, observation.step);
+//			if (useStep) {
-			}
+//				weight *= stepEval.getProbability(p.state, observation.step);
+//			}
 
-			if (useTurn) {
+			// CONTROL!
-				weight *= turnEval.getProbability(p.state, observation.turn, true);
+//			if (useTurn) {
+//				weight *= turnEval.getProbability(p.state, observation.turn, true);
 
-                //set
+//                //set
-                p.state.angularHeadingChange = observation.turn->delta_heading;
+//                p.state.angularHeadingChange = observation.turn->delta_heading;
-			}
+//			}
 
 			// set and accumulate
             p.weight = weight;

code/particles/MyTransition.h

@@ -103,7 +103,7 @@ public:
 
 			// get new destination
 			//const Node3* dst = choice->getTarget(src, p.state, dist_m);
-			p.state.walkState = walker.getDestination(grid, p.state.walkState, control->walked_m, control->headingChange_rad );
+			p.state.walkState = walker.getDestination(grid, p.state.walkState, control->walked_m, control->headingChange_rad, control->currentActivitiy );
 
 			// randomly move the particle within its target grid (box)
 			// (z remains unchanged!)

code/reader/SensorReaderAccel.h

@@ -0,0 +1,35 @@
+#ifndef SENSORREADERACCEL_H
+#define SENSORREADERACCEL_H
+
+#include "../reader/SensorReader.h"
+
+#include <cassert>
+
+class SensorReaderAccel {
+
+public:
+
+	/** get wifi observation data from one CSV entry */
+	static void read(const SensorEntry& se, float dst[3]) {
+
+		std::string tmp = se.data;
+
+		size_t pos1 = tmp.find(';');
+		size_t pos2 = tmp.find(';', pos1+1);
+		size_t pos3 = tmp.find(';', pos2+1);
+
+		if (pos1 == std::string::npos) {throw 1;}
+		if (pos2 == std::string::npos) {throw 1;}
+
+		dst[0] = std::stof( tmp.substr(0, pos1) );
+		dst[1] = std::stof( tmp.substr(pos1+1, pos2-pos1-1) );
+		dst[2] = std::stof( tmp.substr(pos2+1, pos3-pos2-1) );
+
+		int j = 0; (void) j;
+
+	}
+
+};
+
+
+#endif // SENSORREADERACCEL_H