started working on the tex-part

started working on eval-graphics
ned helper methods
tested some new aspects
some fixes and changes
added some graphics
new test-floorplan
many cleanups

code/lukas/StepEvaluation.h

@@ -8,8 +8,8 @@
 #include "StepObservation.h"
 #include <math.h>
 
-static constexpr double mu_walk = 40;
-static constexpr double sigma_walk = 15;
+static constexpr double mu_walk = 90;
+static constexpr double sigma_walk = 30;
 static constexpr double mu_stop = 0;
 static constexpr double sigma_stop = 5;
 
@@ -21,29 +21,49 @@ public:
 
 		return 1;
 
-//        double distance = state.distanceWalkedCM;
+		const float mdlWalked_m = state.walkState.distanceWalked_m;
 
-//        double a = 1.0;
-//	   double mu_distance = 0; //cm
-//		double sigma_distance = 10.0; //cm
+		((MyState&)state).walkState.distanceWalked_m = 0;
 
-//        if(obs->step) {
-//            a = 1.0;
-//			mu_distance = mu_walk;//80.0; //cm
-//			sigma_distance = sigma_walk;//40.0; //cm
-//        }
+		const float stepSize_m = 0.71;
+		const float sensSigma_m = 0.05 + (0.05 * obs->steps);
+		const float sensWalked_m = obs->steps * stepSize_m;
 
-//        else {
-//            a = 0.0;
-//			mu_distance = mu_stop; //cm
-//			sigma_distance = sigma_stop; //cm
-//        }
+		if (obs->steps > 1) {
+			int i = 0;
+			int j = i+1; ++j;
+		}
 
-//        //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);
+		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;
+//		}
+
+//		else {
+//			a = 0.0;
+//			mu_distance = mu_stop;
+//			sigma_distance = sigma_stop;
+//		}
+
+//		//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 p;
     }
 
 };

code/lukas/StepObservation.h

@@ -2,11 +2,14 @@
 #define STEPOBSERVATION_H
 
 struct StepObservation {
-    float ts;
-    bool step;
+
+	float ts;
+
+	int steps = 0;
 
     StepObservation() {;}
-    StepObservation(const float ts) : ts(ts), step(false){;}
+
+	StepObservation(const float ts) : ts(ts), steps(0) {;}
 
 };
 

code/lukas/TurnEvaluation.h

@@ -7,7 +7,7 @@
 //#include <boost/math/special_functions/bessel.hpp>
 #include <math.h>
 
-static constexpr double sigma_heading = 35;
+//static constexpr double sigma_heading = 5;
 
 class TurnEvaluation {
 
@@ -19,6 +19,24 @@ public:
 
 		return 1;
 
+		// 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;
+
+		// proability?
+		const float sigma = 15.0;
+		const double prob = K::NormalDistribution::getProbability(obs->delta_heading, sigma, delta);
+
+		if (prob != prob) {
+			throw 1;
+		}
+
+		return prob;
+
+
+
 //        //Particle's heading change
 //        double delta_heading_particle = state.heading - state.heading_old;
 

code/lukas/TurnObservation.h

@@ -11,7 +11,7 @@ struct TurnObservation {
     float delta_motion; //measured change of motion direction (given by PCA)
 
     TurnObservation() {;}
-    TurnObservation(const float delta_heading, const float motion_angle) : delta_heading(delta_heading), delta_motion(delta_motion) {;}
+	TurnObservation(const float delta_heading, const float motion_angle) : delta_heading(delta_heading), delta_motion(motion_angle) {;}
 
 };