Issue fixed (normalization still not working)

ProbMap.py

@@ -111,7 +111,7 @@ class ProbMap:
         """
         # If Q value after update is small enough to make the probability be zero,
         # it's safe to delete the cell for a better memory usage
-        if val == 700.0:
+        if val == 35.0:
             self.delete_value_from_xy_index(index)
         else:
             self.non_empty_cell[index] = val
@@ -125,7 +125,7 @@ class ProbMap:
         try:
             del self.non_empty_cell[index]
         except KeyError:
-            pass
+            logging.warning(f"{index} does't exist.")
 
     def generate_shareable_v(self, local_measurement):
         # type: (dict) -> dict
@@ -165,8 +165,8 @@ class ProbMap:
         """
 
         def bound_Q(Q):
-            # 700 is big enough to make 1/(1+exp(700)) -> 0 and 1/(1+exp(-700)) -> 1
-            return max(min(Q, 700), -700)
+            # 10 is big enough to make 1/(1+exp(10)) -> 0 and 1/(1+exp(-10)) -> 1
+            return max(min(Q, 10), -10)
 
         # Get the weight of measurements
         weight_local = 1. - (d-1.)/N
@@ -174,7 +174,7 @@ class ProbMap:
 
         # Time decaying factor
         # NOTE Fine tune this param to get a good performance
-        alpha = 5
+        alpha = 8
         T = 0.1
         decay_factor = np.exp(-alpha*T)
         # The diagram below shows the composition of the information for each update
@@ -203,7 +203,7 @@ class ProbMap:
 
         # update all existing grids (Area 1,2,3,4)
         for cell_ind in list(self.non_empty_cell):
-            # Check if it's in area 2 or 4
+            # Check if it's in area 2 or 4 (means we have local measuments about it)
             if cell_ind in local_measurement:
                 v_local = local_measurement[cell_ind]
                 del local_measurement[cell_ind]
@@ -267,7 +267,8 @@ class ProbMap:
         Args:
             threshold (float): Values higher than this will be returned
         """
-        lower_threshold = 0.2
+        logging.debug(f"{self.non_empty_cell}")
+        lower_threshold = 0.1
         if threshold < 0.5:
             # shrink the lower threshold value
             lower_threshold *= threshold
@@ -293,20 +294,24 @@ class ProbMap:
                     self.prob_map[cell_ind] = normed_prob
                 else:
                     del self.prob_map[cell_ind]
-
-                if normed_prob < lower_threshold:
-                    # keep some uncertainty between the lower and upper thresholds
-                    self.delete_value_from_xy_index(cell_ind)
+                # if normed_prob <= lower_threshold*8:
+                #     # keep some uncertainty between the lower and upper thresholds
+                #     self.delete_value_from_xy_index(cell_ind)
         else:
             for cell_ind in list(self.non_empty_cell):
                 value = self.non_empty_cell[cell_ind]
+                # logging.debug(f"PROB: {value}")
                 # Decode the probability value
                 prob = 1./(1.+np.exp(value))
                 if prob >= threshold:
                     self.prob_map[cell_ind] = prob
-                elif prob < lower_threshold:
+                    if prob >= 0.99999:
+                        logging.warning(f"GOT 1!!! {prob} {value}")
+                if prob < lower_threshold:
+                    # logging.debug(f"Deleting {cell_ind},{prob}")
                     # keep some uncertainty between the lower and upper thresholds
                     self.delete_value_from_xy_index(cell_ind)
+                    
 
     def get_target_est(self, threshold, normalization=False):
         """Get all targets' estimated position
@@ -335,4 +340,4 @@ class ProbMapData:
         self.myid = -1
         self.type = 'n'
         self.grid_ind = list()
-        self.values = list()
+        self.values = list()

Robot.py

@@ -4,37 +4,41 @@ import numpy as np
 
 
 class Robot:
-    def __init__(self, simulator,  name: str, id: int, position: np.array, rate: int) -> None:
+    def __init__(self, simulator,  name: str, id: int, position: np.array) -> None:
         """The base Robot class
 
         Parameters
         ----------
+        simulator: Simsim
+            Host simulator object
         name : str
             Robot's name
         id : int
             Robot's ID
         position : np.array
-            Robot's start position
-        rate : int
-            Simulator's update rate
+            Robot's starting position
         """
         # host simulator
         self.simulator = simulator
         self.name = name
         self.id = id
+        self.log_head = f"{self.name}_{self.id}"
         self.position = position
+        self.ang = np.random.random()*360
         self.desierd_position = self.position
         self.in_position = True
         self.speed = 0.0
-        self.rate = rate
+        self.rate = self.simulator.rate
         self.max_speed = 100.0  # m/s
 
     def waypoint_ctrl(self, speed=None, desierd_pos: np.array = None):
         # if got new waypoint, update
         if not np.any(desierd_pos == None):
+            logging.debug(f"{self.name}{self.id} set new desierd position")
             self.desierd_position = desierd_pos
 
-        if speed == None:
+        if speed is None:
+            logging.debug(f"{self.name}_{self.id} set to max speed")
             self.speed = self.max_speed
         else:
             if speed > self.max_speed:
@@ -42,8 +46,10 @@ class Robot:
             self.speed = speed
 
         if np.any(self.position != self.desierd_position):
+            logging.debug(
+                f"{self.name}_{self.id} moving to {self.desierd_position}")
             self.in_position = False
-            one_time_step_length = speed/self.rate
+            one_time_step_length = self.speed/self.rate
             difference = self.desierd_position - self.position
             distance = np.linalg.norm(difference)
             # if it's close enough to the goal position, make it in position
@@ -54,6 +60,8 @@ class Robot:
                 # move to the goal point at the given speed
                 direction = difference/distance
                 self.position = self.position + direction*self.speed/self.rate
+                # print("MOVED")
+        return self.in_position
 
     def job(self):
         self.waypoint_ctrl()

Simsim.py

@@ -35,13 +35,14 @@ class Simsim:
         self.topics = self.Topics()
 
         plt.ion()
-        plt.axis('equal')
+
         self.fig, (self.plt_sim, self.plt_pm) = plt.subplots(
             1, 2, figsize=(10, 5), dpi=160)
+        plt.axis('equal')
 
     def add_tracker(self, name, position, sensor_rad):
         tracker = Tracker(self, name, len(self.trackers),
-                          position, sensor_rad, self.rate)
+                          position, sensor_rad)
         self.trackers.append(tracker)
 
     def add_edges(self, edges):
@@ -51,7 +52,7 @@ class Simsim:
             self.trackers[e[1]].neighbor.add(e[0])
 
     def add_target(self, name, position):
-        target = Target(self, name, len(self.targets), position, self.rate)
+        target = Target(self, name, len(self.targets), position)
         self.targets.append(target)
 
     def _update_all(self):
@@ -61,7 +62,6 @@ class Simsim:
             tracker.job()
         for target in self.targets:
             target.job()
-        pass
 
     def run(self, log_lvl=logging.WARN, ground_truth=False):
         logging.basicConfig(format='%(asctime)s.%(msecs)03d %(levelname)s: %(message)s',
@@ -72,8 +72,8 @@ class Simsim:
             self.plt_pm.cla()
             self.plt_sim.set_xlim(0, self.map_size[0])
             self.plt_sim.set_ylim(0, self.map_size[1])
-            self.plt_pm.set_xlim(0, self.map_size[0])
-            self.plt_pm.set_ylim(0, self.map_size[1])
+            self.plt_pm.set_xlim(2000, 4000)
+            self.plt_pm.set_ylim(2000, 4000)
 
             if ground_truth:
                 for target in self.targets:
@@ -98,11 +98,12 @@ class Simsim:
 
                 for est in t.target_estimates:
                     det_pos = est[0:2]
-                    det_abs_pos = det_pos+t.position
-                    self.plt_sim.scatter(det_abs_pos[0], det_abs_pos[1],
+                    # det_abs_pos = det_pos+t.position
+                    self.plt_sim.scatter(det_pos[0], det_pos[1],
                                          marker='^', s=2)
                     for ind in t.prob_map.prob_map:
-                        new_ind = np.array(ind) - self.map_size + t.position
+                        new_ind = np.array(ind) # - self.map_size
                         self.plt_pm.scatter(
                             new_ind[0], new_ind[1], marker='s', s=1, c='r', alpha=t.prob_map.prob_map[ind])
+                        self.plt_pm.annotate(f"{t.prob_map.prob_map[ind]:.3e}", (new_ind[0], new_ind[1]+5))
             plt.pause(1/self.rate)

main.py

@@ -5,16 +5,17 @@ import logging
 if __name__ == "__main__":
     s = Simsim()
     s.rate = 5
-    s.add_tracker('tracker', np.array([200, 100]), sensor_rad=300)
-    s.add_tracker('tracker', np.array([100, 100]), sensor_rad=200)
-    s.add_tracker('tracker', np.array([500, 380]), sensor_rad=200)
-    s.add_tracker('tracker', np.array([700, 580]), sensor_rad=200)
-    s.add_target('tgt', np.array([50, 50]))
-    s.add_target('tgt', np.array([140, 160]))
-    s.add_target('tgt', np.array([180, 500]))
+    s.add_tracker('tracker', np.array([500, 300]), sensor_rad=250)
+    s.add_tracker('tracker', np.array([300, 500]), sensor_rad=250)
+    s.add_tracker('tracker', np.array([700, 500]), sensor_rad=250)
+    # s.add_tracker('tracker', np.array([700, 580]), sensor_rad=200)
+    # s.add_tracker('tracker', np.array([620, 220]), sensor_rad=450)
+    # s.add_target('tgt', np.array([150, 50]))
+    s.add_target('tgt', np.array([500, 490]))
+    # s.add_target('tgt', np.array([180, 500]))
     s.add_target('tgt', np.array([500, 300]))
     s.add_target('tgt', np.array([300, 500]))
-    s.add_target('tgt', np.array([630, 500]))
-    s.add_edges([[0, 1], [0, 2], [3, 2], [1, 3]])
-    # s.add_edges([[0,1]])
-    s.run(logging.DEBUG, ground_truth=True)
+    # s.add_target('tgt', np.array([630, 500]))
+    # s.add_edges([[0, 1], [0, 2], [3, 2], [1, 3], [1, 4], [3, 4]])
+    s.add_edges([[0, 1], [1, 2], [0, 2]])
+    s.run(logging.DEBUG, ground_truth=True)

target.py

@@ -7,9 +7,10 @@ class Target(Robot):
         super().__init__(simulator, name, id, position)
 
     def job(self):
+        # return 0
         if self.waypoint_ctrl():
             self.ang = (self.ang + np.deg2rad(15)) % 360
-            # self.waypoint_ctrl(desierd_pos=[1000,1000])
-            self.waypoint_ctrl(speed=20, desierd_pos=(self.position-np.dot([0, 10],
-                                                                           np.asarray([[np.cos(self.ang), -np.sin(self.ang)],
-                                                                                       [np.sin(self.ang), np.cos(self.ang)]]))))
+            rot_mat = np.asarray([[np.cos(self.ang), -np.sin(self.ang)],
+                                  [np.sin(self.ang), np.cos(self.ang)]])
+            self.waypoint_ctrl(speed=20, desierd_pos=(self.position
+                                                      - np.dot([0, 10], rot_mat)))

tracker.py

@@ -28,6 +28,8 @@ class Sensor:
                 noise = np.random.normal(loc=0, scale=std_dev, size=2)
                 confidence = sum(st.norm.pdf(
                     noise, loc=0, scale=std_dev)*2)/2*std_dev
+                if confidence <= 0.55:
+                    confidence = 0.55
                 detection = detection + noise
                 distance = np.linalg.norm(detection)
                 if distance > self.coverage_radius:
@@ -35,22 +37,22 @@ class Sensor:
                 detection = np.append(detection, confidence)
                 detections.append(detection)
                 logging.debug(
-                    f"Noisy {self.tracker.name}{self.tracker.id} detection: {detection}")
+                    f"Noisy {self.tracker.log_head} Detection: {detection}")
         return detections
 
 
 class Tracker(Robot):
-    def __init__(self, simulator, name: str, id: int, position: np.array, coverage_radius, rate: int) -> None:
-        super().__init__(simulator, name, id, position, rate)
+    def __init__(self, simulator, name: str, id: int, position: np.array, coverage_radius) -> None:
+        super().__init__(simulator, name, id, position)
         self.sensor = Sensor(self, coverage_radius)
         self.neighbor = set()
 
         self.area_width = 2000  # meter
         self.area_height = 2000  # meter
-        self.resolution = 1.0  # meter
+        self.resolution = 0.5  # meter
         self.prob_map = ProbMap(self.area_width, self.area_height, self.resolution,
-                                center_x=0.0, center_y=0.0, init_val=0.01,
-                                false_alarm_prob=0.05)
+                                center_x=0.0, center_y=0.0, init_val=0.05,
+                                false_alarm_prob=0.01)
 
         self.observations = dict()  # type: dict[tuple]
         self.shareable_v = ProbMapData()
@@ -109,12 +111,14 @@ class Tracker(Robot):
         output_detection = dict()
         id_counter = 0
         for det in detections:
-            output_detection[id_counter] = det
+            transformed_detection = det+np.append(self.position, 0)
+            output_detection[id_counter] = transformed_detection
             id_counter += 1
         self.observations = output_detection
 
     def job(self):
         self.sensing()
+        logging.debug(f"{self.log_head} OBSERVATION: {self.observations}")
         shareable_v = self.prob_map.generate_shareable_v(
             self.observations)
 
@@ -144,6 +148,7 @@ class Tracker(Robot):
         self.prob_map.consensus(neighbors_map)
 
         self.target_estimates = self.prob_map.get_target_est(
-            0.8, normalization=False)
-        print(self.prob_map.prob_map)
+            0.6, normalization=True)
+        logging.debug(
+            f"{self.name}_{self.id} ProbMap: {self.prob_map.prob_map}")
         # print(target_estimates)