experimental stuff... tryed dynamic step length using the barometric height

code/CMakeLists.txt.user

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 3.6.0, 2016-03-29T16:54:26. -->
+<!-- Written by QtCreator 3.6.0, 2016-04-01T17:34:46. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>

code/eval/FixedLagEvalBase.h

@@ -193,6 +193,9 @@ public:
         //stats file
         std::ofstream statsout("/tmp/unsmoothed_" + runName + ".stats");
 
+        //heading
+        double currentHeadingGivenByLukas = 0.0;
+
         // process each single sensor reading
         while(sr->hasNext()) {
 
@@ -251,6 +254,8 @@ public:
                 obs.turn->delta_motion += _to.delta_motion;
                 ctrl.headingChange_rad = Angle::degToRad(obs.turn->delta_heading);
 
+                currentHeadingGivenByLukas = obs.turn->delta_heading;
+
             }
 
 
@@ -267,6 +272,11 @@ public:
                 const MyState est = pf->update(&ctrl, obs);
                 const Point3 curEst = est.pCur;
 
+                //EXPERIMENTAL: Set all Particle Angles to the estimated angle of the particle set
+//                if(cnt % 30 == 0){
+//                    pf->setAngle(est.avgAngle * 3.14159265359 / 180);
+//                }
+
                 // error calculation. compare ground-truth to estimation
                 const int offset = 0;
                 const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
@@ -336,6 +346,18 @@ public:
                     }
                     vis.gp << "set label 111 '" << ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
 
+                    //double avgAngleRad = estBF.avgAngle * 180/3.14159265359;
+
+                    //std::cout << "Measurement: "<< std::fmod((-(obs.orientation.values[0] * 180/3.14159265359) + 720 + 30), 360)  << std::endl;
+
+
+                    //std::cout << "Measurement: "<< std::fmod(((obs.orientation.values[0] * 180/3.14159265359) + 720 + 60), 360)  << std::endl;
+
+                    std::cout << "MeasurementLukas: " << currentHeadingGivenByLukas << std::endl;
+                    std::cout << "EstimationS: " << estBF.avgAngle << std::endl;
+                    std::cout << "EstimationF: " << est.avgAngle << std::endl;
+                    //vis.gp << "set label 113 ' EstAngle:" << avgAngleRad << "' at screen 0.1,0.15\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);

code/eval/SmoothingEval1.h

@@ -19,7 +19,7 @@
 
 #include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
 #include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
-#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
+#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverageWithAngle.h>
 
 class SmoothingEval1 : public FixedLagEvalBase {
 
@@ -45,7 +45,7 @@ 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::ParticleFilterEstimationWeightedAverageWithAngle<MyState>>(new K::ParticleFilterEstimationWeightedAverageWithAngle<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)));
 
@@ -100,14 +100,14 @@ public:
         //Smoothing Variables
         smoothing_walk_mu = 0.7;
         smoothing_walk_sigma = 0.5;
-        smoothing_heading_sigma = 15.0;
+        smoothing_heading_sigma = 5.0;
         smoothing_baro_sigma = 0.05;
 
         bool smoothing_resample = false;
         smoothing_time_delay = 1;
 
         //Smoothing using Simple Trans
-        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
+        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>>(new K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>()));
         if(smoothing_resample)
             bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
         bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );

code/particles/MyEvaluation.h

@@ -79,7 +79,7 @@ public:
 				weight *= turnEval.getProbability(p.state, observation.turn, true);
 
                 //set
-                p.state.measurement_angle = observation.turn->delta_heading;
+                p.state.angularHeadingChange = observation.turn->delta_heading;
 			}
 
 			// set and accumulate

code/particles/MyState.h

@@ -29,8 +29,10 @@ struct MyState {
     // save last hPa measurement for the smoothing process
     double measurement_pressure;
 
-    // save last angle measurement for the smoothing process
+    // save last angularHeadingChangefor the smoothing process in Degree
-    double measurement_angle;
+    double angularHeadingChange;
+
+    double avgAngle;
 
 	//int distanceWalkedCM;
 

code/particles/smoothing/MySmoothingTransitionExperimental.h

@@ -29,7 +29,8 @@ public:
 	 * @param fp the underlying floorplan
 	 */
     MySmoothingTransitionExperimental() :
-        distWalk(smoothing_walk_mu, smoothing_walk_sigma) {
+        distWalk(smoothing_walk_mu, smoothing_walk_sigma)
+    {
         distWalk.setSeed(4321);
 	}
 
@@ -75,23 +76,34 @@ public:
                 //!!!distance kann hier zu groß werden!!!
                 const double distance_m = p2->state.pCur.getDistance(p1->state.pCur) / 100.0;
 
+
+
                 //get distance walked and getProb using the walking model
                 //double distDijkstra_m = ((GRID_DISTANCE_CM / 100.0) * (8 - 1));
                 const double distProb = distWalk.getProbability(distance_m);
 
 
                 //getProb using the angle(heading) between src and dst
-                double angle = 0.0;
+//                double angle = 0.0;
-                if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){
+//                if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){
-                    angle = Angle::getDEG_360(p2->state.pCur.x, p2->state.pCur.y, p1->state.pCur.x, p1->state.pCur.y);
+//                    angle = Angle::getDEG_360(p2->state.pCur.x, p2->state.pCur.y, p1->state.pCur.x, p1->state.pCur.y);
-                }
+//                }
 
-                const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle);
+//                const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle);
+
+
+                // is the heading change similiar to the measurement?
+                double p2AngleDeg = p2->state.walkState.heading.getRAD() * 180/3.14159265359;
+                double p1AngleDeg = p1->state.walkState.heading.getRAD() * 180/3.14159265359;
+
+                double diffDeg = p2AngleDeg - p1AngleDeg;
+                const double headingProb = K::NormalDistribution::getProbability(p1->state.angularHeadingChange, smoothing_heading_sigma, diffDeg);
 
                 //assert(headingProb != 0.0);
                 //assert(distProb != 0.0);
 
 
+
                 //check how near we are to the measurement
                 double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa);
 
@@ -102,7 +114,7 @@ public:
 
                 if(distance_m != distance_m) {throw "detected NaN";}
                 if(distProb != distProb) {throw "detected NaN";}
-                if(angle != angle) {throw "detected NaN";}
+//                if(angle != angle) {throw "detected NaN";}
                 if(headingProb != headingProb) {throw "detected NaN";}
                 if(floorProb != floorProb) {throw "detected NaN";}
                 if(floorProb == 0) {throw "detected NaN";}