From f66fa16bf5478ca6d95ad309ff64becaa867d8c1 Mon Sep 17 00:00:00 2001
From: Sebastian Mattheis <sebastian.mattheis@bmw-carit.de>
Date: Wed, 7 Jun 2017 17:53:37 +0200
Subject: [PATCH] Bugfixes: emission oscillation filter and computation of
 emission probability.

Change-Id: Id6f2be5cd156445a2d6e20de80ec9a3e4821792c
---
 pom.xml                                       |  2 +-
 .../bmwcarit/barefoot/matcher/Matcher.java    | 23 ++++++++++---------
 .../barefoot/matcher/MatcherTest.java         |  2 +-
 3 files changed, 14 insertions(+), 13 deletions(-)

diff --git a/pom.xml b/pom.xml
index 3ac5d5e8..b7aa61b0 100755
--- a/pom.xml
+++ b/pom.xml
@@ -2,7 +2,7 @@
 	<modelVersion>4.0.0</modelVersion>
 	<groupId>com.bmw-carit</groupId>
 	<artifactId>barefoot</artifactId>
-	<version>0.1.0</version>
+	<version>0.1.1</version>
 	<build>
 		<plugins>
 			<plugin>
diff --git a/src/main/java/com/bmwcarit/barefoot/matcher/Matcher.java b/src/main/java/com/bmwcarit/barefoot/matcher/Matcher.java
index 8ecd4cef..0a00a5cd 100644
--- a/src/main/java/com/bmwcarit/barefoot/matcher/Matcher.java
+++ b/src/main/java/com/bmwcarit/barefoot/matcher/Matcher.java
@@ -29,6 +29,7 @@
 
 import com.bmwcarit.barefoot.markov.Filter;
 import com.bmwcarit.barefoot.markov.KState;
+import com.bmwcarit.barefoot.road.Heading;
 import com.bmwcarit.barefoot.roadmap.Distance;
 import com.bmwcarit.barefoot.roadmap.Road;
 import com.bmwcarit.barefoot.roadmap.RoadMap;
@@ -178,23 +179,26 @@ protected Set<Tuple<MatcherCandidate, Double>> candidates(Set<MatcherCandidate>
         for (RoadPoint point : points) {
             map.put(point.edge().id(), point);
         }
-
         for (MatcherCandidate predecessor : predecessors) {
             RoadPoint point = map.get(predecessor.point().edge().id());
-            if (point != null && point.fraction() < predecessor.point().fraction()) {
+            if (point != null && point.edge() != null
+                    && spatial.distance(point.geometry(),
+                            predecessor.point().geometry()) < getSigma()
+                    && ((point.edge().heading() == Heading.forward
+                            && point.fraction() < predecessor.point().fraction())
+                            || (point.edge().heading() == Heading.backward
+                                    && point.fraction() > predecessor.point().fraction()))) {
                 points.remove(point);
                 points.add(predecessor.point());
             }
         }
 
-        Set<Tuple<MatcherCandidate, Double>> candidates =
-                new HashSet<>();
-
+        Set<Tuple<MatcherCandidate, Double>> candidates = new HashSet<>();
         logger.debug("{} ({}) candidates", points.size(), points_.size());
 
         for (RoadPoint point : points) {
             double dz = spatial.distance(sample.point(), point.geometry());
-            double emission = 1 / sqrt_2pi_sig2 * Math.exp((-1) * dz / (2 * sig2));
+            double emission = 1 / sqrt_2pi_sig2 * Math.exp((-1) * dz * dz / (2 * sig2));
             if (!Double.isNaN(sample.azimuth())) {
                 double da = sample.azimuth() > point.azimuth()
                         ? Math.min(sample.azimuth() - point.azimuth(),
@@ -204,7 +208,6 @@ protected Set<Tuple<MatcherCandidate, Double>> candidates(Set<MatcherCandidate>
                 emission *= Math.max(1E-2, 1 / sqrt_2pi_sigA * Math.exp((-1) * da / (2 * sigA)));
             }
 
-
             MatcherCandidate candidate = new MatcherCandidate(point);
             candidates.add(new Tuple<>(candidate, emission));
 
@@ -256,8 +259,7 @@ protected Map<MatcherCandidate, Map<MatcherCandidate, Tuple<MatcherTransition, D
             scheduler.spawn(new Task() {
                 @Override
                 public void run() {
-                    Map<MatcherCandidate, Tuple<MatcherTransition, Double>> map =
-                            new HashMap<>();
+                    Map<MatcherCandidate, Tuple<MatcherTransition, Double>> map = new HashMap<>();
                     Stopwatch sw = new Stopwatch();
                     sw.start();
                     Map<RoadPoint, List<Road>> routes =
@@ -288,8 +290,7 @@ public void run() {
                         double transition = (1 / beta) * Math.exp(
                                 (-1.0) * Math.max(0, route.cost(new TimePriority()) - base) / beta);
 
-                        map.put(candidate, new Tuple<>(
-                                new MatcherTransition(route), transition));
+                        map.put(candidate, new Tuple<>(new MatcherTransition(route), transition));
 
                         logger.trace("{} -> {} {} {} {}", predecessor.id(), candidate.id(), base,
                                 route.length(), transition);
diff --git a/src/test/java/com/bmwcarit/barefoot/matcher/MatcherTest.java b/src/test/java/com/bmwcarit/barefoot/matcher/MatcherTest.java
index 000530e3..69498c0f 100644
--- a/src/test/java/com/bmwcarit/barefoot/matcher/MatcherTest.java
+++ b/src/test/java/com/bmwcarit/barefoot/matcher/MatcherTest.java
@@ -123,7 +123,7 @@ private void assertCandidate(Tuple<MatcherCandidate, Double> candidate, Point sa
         double l = spatial.distance(i, sample);
         double sig2 = Math.pow(5d, 2);
         double sqrt_2pi_sig2 = Math.sqrt(2d * Math.PI * sig2);
-        double p = 1 / sqrt_2pi_sig2 * Math.exp((-1) * l / (2 * sig2));
+        double p = 1 / sqrt_2pi_sig2 * Math.exp((-1) * l * l / (2 * sig2));
 
         assertEquals(f, candidate.one().point().fraction(), 10E-6);
         assertEquals(p, candidate.two(), 10E-6);