began putting everything together

code/lukas/StepEvaluation.h

@@ -8,10 +8,10 @@
 #include "StepObservation.h"
 #include <math.h>
 
-static double mu_walk = 40;
-static double sigma_walk = 15;
-static double mu_stop = 0;
-static double sigma_stop = 5;
+static constexpr double mu_walk = 40;
+static constexpr double sigma_walk = 15;
+static constexpr double mu_stop = 0;
+static constexpr double sigma_stop = 5;
 
 class StepEvaluation {
 
@@ -19,29 +19,31 @@ public:
 
     double getProbability(const MyState& state, const StepObservation* obs) const {
 
-        double distance = state.distanceWalkedCM;
+		return 1;
 
-        double a = 1.0;
-	   double mu_distance = 0; //cm
-		double sigma_distance = 10.0; //cm
+//        double distance = state.distanceWalkedCM;
 
-        if(obs->step) {
-            a = 1.0;
-			mu_distance = mu_walk;//80.0; //cm
-			sigma_distance = sigma_walk;//40.0; //cm
-        }
+//        double a = 1.0;
+//	   double mu_distance = 0; //cm
+//		double sigma_distance = 10.0; //cm
 
-        else {
-            a = 0.0;
-			mu_distance = mu_stop; //cm
-			sigma_distance = sigma_stop; //cm
-        }
+//        if(obs->step) {
+//            a = 1.0;
+//			mu_distance = mu_walk;//80.0; //cm
+//			sigma_distance = sigma_walk;//40.0; //cm
+//        }
 
-        //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);
+//        else {
+//            a = 0.0;
+//			mu_distance = mu_stop; //cm
+//			sigma_distance = sigma_stop; //cm
+//        }
 
-        return p;
+//        //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;
     }
 
 };

code/lukas/TurnEvaluation.h

@@ -4,10 +4,10 @@
 
 #include "../particles/MyState.h"
 #include "TurnObservation.h"
-#include <boost/math/special_functions/bessel.hpp>
+//#include <boost/math/special_functions/bessel.hpp>
 #include <math.h>
 
-static double sigma_heading = 35;
+static constexpr double sigma_heading = 35;
 
 class TurnEvaluation {
 
@@ -17,55 +17,33 @@ public:
 
     double getProbability(const MyState& state, const TurnObservation* obs, bool simple = false) const {
 
-        //Particle's heading change
-        double delta_heading_particle = state.heading - state.heading_old;
+		return 1;
+
+//        //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;
-        }
+//        //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) {
+//        //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;
+//			double sigma_delta_heading = sigma_heading;
 
-            const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle);
+//            const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle);
 
 
-            return p;
-        }
-
-        //use the von Mises distribution
-        else {
-            //Here some calculations must be done in rad
-
-            double delta_heading_obs_rad = obs->delta_heading * 3.14159265359 / 180.0;
-            double delta_motion_rad = obs -> delta_motion * 3.14159265359 / 180.0;
-
-            //Equation for estimating kappa value of von Mises distribution
-            //empirically estimated
-            double kappa = 0.0;
-
-            kappa = 5.0 / exp(2 * delta_motion_rad);
-
-            double delta_heading_particle_rad = delta_heading_particle * 3.14159265359 / 180.0;
-
-
-
-            //pdf von mises distribution (http://en.wikipedia.org/wiki/Von_Mises_distribution)
-            const double p = exp(kappa * cos(delta_heading_obs_rad - delta_heading_particle_rad)) / (2.0 * 3.14159265359 * boost::math::cyl_bessel_i(0, kappa));
-
-            return p;
-
-        }
+//            return p;
+//	   // }
 
     }
 

code/lukas/TurnReader.h

@@ -1,7 +1,7 @@
 #ifndef TURNREADER_H
 #define TURNREADER_H
 
-#include "../SensorReaderTurn.h"
+#include "../reader/SensorReaderTurn.h"
 #include "TurnObservation.h"
 
 class TurnReader {