From ee128f1d57e461ead037ab25196726214263571c Mon Sep 17 00:00:00 2001
From: Chipe1 <vedant.cacklur@students.iiit.ac.in>
Date: Thu, 3 Aug 2017 12:52:58 +0530
Subject: [PATCH] Added Monte Carlo localization

---
 probability.py            | 66 +++++++++++++++++++++++++++++++++++++++
 tests/test_probability.py | 62 ++++++++++++++++++++++++++++++++++++
 2 files changed, 128 insertions(+)

diff --git a/probability.py b/probability.py
index 347efc7bd..5a5870f64 100644
--- a/probability.py
+++ b/probability.py
@@ -649,3 +649,69 @@ def particle_filtering(e, N, HMM):
     s = weighted_sample_with_replacement(N, s, w)
 
     return s
+
+# _________________________________________________________________________
+## TODO: Implement continous map for MonteCarlo similar to Fig25.10 from the book
+
+class MCLmap:
+    """Map which provides probability distributions and sensor readings.
+    Consists of discrete cells which are either an obstacle or empty"""
+    def __init__(self, m):
+        self.m = m
+        self.nrows = len(m)
+        self.ncols = len(m[0])
+        # list of empty spaces in the map
+        self.empty = [[i, j] for i in range(self.nrows) for j in range(self.ncols) if not m[i][j]]
+
+    def sample(self):
+        """Returns a random kinematic state possible in the map"""
+        pos = random.choice(self.empty)
+        # 0N 1E 2S 3W
+        orient = random.choice(range(4))
+        kin_state = pos + [orient]
+        return kin_state
+
+    def ray_cast(self, sensor_num, kin_state):
+        """Returns distace to nearest obstacle or map boundary in the direction of sensor"""
+        pos = kin_state[:2]
+        orient = kin_state[2]
+        # sensor layout when orientation is 0 (towards North)
+        #  0
+        # 3R1
+        #  2
+        delta = [(sensor_num%2 == 0)*(sensor_num - 1), (sensor_num%2 == 1)*(2 - sensor_num)]
+        # sensor direction changes based on orientation
+        for _ in range(orient):
+            delta = [delta[1], -delta[0]]
+        range_count = 0
+        while (0 <= pos[0] < self.nrows) and (0 <= pos[1] < self.nrows) and (not self.m[pos[0]][pos[1]]):
+            pos = vector_add(pos, delta)
+            range_count += 1
+        return range_count
+
+
+def monte_carlo_localization(a, z, N, P_motion_sample, P_sensor, m, S=None):
+    """Monte Carlo localization algorithm from Fig 25.9"""
+
+    def ray_cast(sensor_num, kin_state, m):
+        return m.ray_cast(sensor_num, kin_state)
+
+    M = len(z)
+    W = [0]*N
+    S_ = [0]*N
+    W_ = [0]*N
+    v = a['v']
+    w = a['w']
+
+    if S is None:
+        S = [m.sample() for _ in range(N)]
+
+    for i in range(N):
+        S_[i] = P_motion_sample(S[i], v, w)
+        W_[i] = 1
+        for j in range(M):
+            z_ = ray_cast(j, S_[i], m)
+            W_[i] = W_[i] * P_sensor(z[j], z_)
+
+    S = weighted_sample_with_replacement(N, S_, W_)
+    return S
diff --git a/tests/test_probability.py b/tests/test_probability.py
index cfffee5bd..2ec860876 100644
--- a/tests/test_probability.py
+++ b/tests/test_probability.py
@@ -168,6 +168,68 @@ def test_particle_filtering():
     # XXX 'A' and 'B' are really arbitrary names, but I'm letting it stand for now
 
 
+def test_monte_carlo_localization():
+    ## TODO: Add tests for random motion/inaccurate sensors
+    random.seed('aima-python')
+    m = MCLmap([[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 0],
+                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 1, 0],
+                [1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 0, 1, 1, 0],
+                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 1, 0],
+                [0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0],
+                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 1, 0],
+                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 1, 0],
+                [0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 0, 1, 1, 0],
+                [0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
+                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0],
+                [0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 0, 0, 1, 0]])
+
+    def P_motion_sample(kin_state, v, w):
+        """Sample from possible kinematic states.
+        Returns from a single element distribution (no uncertainity in motion)"""
+        pos = kin_state[:2]
+        orient = kin_state[2]
+        
+        # for simplicity the robot first rotates and then moves
+        orient = (orient + w)%4
+        for _ in range(orient):
+            v = [v[1], -v[0]]
+        pos = list(vector_add(pos, v))
+        return pos + [orient]
+
+    def P_sensor(x, y):
+        """Conditional probability for sensor reading"""
+        # Need not be exact probability. Can use a scaled value.
+        if x == y:
+            return 0.8
+        elif abs(x - y) <= 2:
+            return 0.05
+        else:
+            return 0
+
+    from utils import print_table
+    a = {'v': [0, 0], 'w': 0}
+    z = [2, 4, 1, 6]
+    S = monte_carlo_localization(a, z, 1000, P_motion_sample, P_sensor, m)
+    grid = [[0]*17 for _ in range(11)]
+    for x, y, _ in S:
+        if 0 <= x < 11 and 0 <= y < 17:
+            grid[x][y] += 1
+    print("GRID:")
+    print_table(grid)
+
+    a = {'v': [0, 1], 'w': 0}
+    z = [2, 3, 5, 7]
+    S = monte_carlo_localization(a, z, 1000, P_motion_sample, P_sensor, m, S)
+    grid = [[0]*17 for _ in range(11)]
+    for x, y, _ in S:
+        if 0 <= x < 11 and 0 <= y < 17:
+            grid[x][y] += 1
+    print("GRID:")
+    print_table(grid)
+
+    assert grid[6][7] > 700
+
+
 # The following should probably go in .ipynb:
 
 """