added fixed interval smoothing

code/particles/MyEvaluation.h

@@ -11,6 +11,8 @@
 #include "../lukas/StepEvaluation.h"
 #include "../lukas/TurnEvaluation.h"
 
+#include <atomic>
+
 class MyEvaluation : public K::ParticleFilterEvaluation<MyState, MyObservation> {
 
 private:
@@ -47,6 +49,11 @@ public:
 		double sum = 0;
 		for (K::Particle<MyState>& p : particles) {
 
+//		std::atomic<double> sum(0);
+//		#pragma omp parallel for
+//		for (int i = 0; i < (int) particles.size(); ++i) {
+//			K::Particle<MyState>& p = particles[i];
+
 			double weight = 1.0;
 
 			if (useWifi) {
@@ -55,6 +62,9 @@ public:
 
 			if (useBaro && observation.barometer) {
 				weight *= barometerEval.getProbability(p.state, observation.barometer);
+
+                // set
+                p.state.measurement_pressure = observation.barometer->hpa;
 			}
 
 			if (useIB) {

code/particles/MyState.h

@@ -23,6 +23,12 @@ struct MyState {
 	// the grid-walk state
 	GridWalkState<MyGridNode> walkState;
 
+    // cumulative heading
+    double cumulativeHeading;
+
+    // save last hPa measurement for the smoothing process
+    double measurement_pressure;
+
 	//int distanceWalkedCM;
 
 

code/particles/MyTransition.h

@@ -87,6 +87,9 @@ public:
 
 			// update the old heading and the other old values
 			//p.state.walkState.heading = p.state.heading;
+            if(!(p.state.pOld == p.state.pCur)){
+                p.state.cumulativeHeading = Angle::getDEG_360(p.state.pOld.x, p.state.pOld.y, p.state.pCur.x, p.state.pCur.y);
+            }
 			p.state.pOld = p.state.pCur;