initial commit before ownership transfer

code/lukas/TurnEvaluation.h

@@ -0,0 +1,76 @@
+#ifndef TURNEVALUATION_H
+#define TURNEVALUATION_H
+
+
+#include "../particles/MyState.h"
+#include "TurnObservation.h"
+#include <boost/math/special_functions/bessel.hpp>
+#include <math.h>
+
+static double sigma_heading = 35;
+
+class TurnEvaluation {
+
+    //All calculations use degree not rad!!!
+
+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;
+
+
+        //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) {
+
+			double sigma_delta_heading = sigma_heading;
+
+            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;
+
+        }
+
+    }
+
+};
+
+
+
+#endif // TURNEVALUATION_H