diff --git a/people_tracking/CMakeLists.txt b/people_tracking/CMakeLists.txt
index 3727d4b..d9a6be7 100644
--- a/people_tracking/CMakeLists.txt
+++ b/people_tracking/CMakeLists.txt
@@ -21,6 +21,7 @@ add_message_files(
    detect_person.msg
    DetectedPerson.msg
    ColourCheckedTarget.msg
+   ColourTarget.msg
 )
 
 add_service_files(
diff --git a/people_tracking/msg/ColourTarget.msg b/people_tracking/msg/ColourTarget.msg
new file mode 100644
index 0000000..9ac5711
--- /dev/null
+++ b/people_tracking/msg/ColourTarget.msg
@@ -0,0 +1,8 @@
+float64 time
+int32 nr_batch
+int32 nr_persons
+
+float32[] colour_vectors
+int32[] x_positions
+int32[] y_positions
+int32[] z_positions
\ No newline at end of file
diff --git a/people_tracking/src/colour_check.py b/people_tracking/src/colour_check.py
index b886e18..89fe222 100755
--- a/people_tracking/src/colour_check.py
+++ b/people_tracking/src/colour_check.py
@@ -8,6 +8,10 @@
 from sensor_msgs.msg import Image
 from people_tracking.msg import ColourCheckedTarget
 from people_tracking.msg import DetectedPerson
+from people_tracking.msg import ColourTarget
+
+from rospy.numpy_msg import numpy_msg
+
 
 NODE_NAME = 'HoC'
 TOPIC_PREFIX = '/hero/'
@@ -20,7 +24,7 @@ def __init__(self) -> None:
         rospy.init_node(NODE_NAME, anonymous=True)
         self.subscriber = rospy.Subscriber(TOPIC_PREFIX + 'person_detections', DetectedPerson, self.callback,
                                            queue_size=1)
-        self.publisher = rospy.Publisher(TOPIC_PREFIX + 'HoC', ColourCheckedTarget, queue_size=2)
+        self.publisher = rospy.Publisher(TOPIC_PREFIX + 'HoC', ColourTarget, queue_size=2)
         # self.publisher_debug = rospy.Publisher(TOPIC_PREFIX + 'debug/HoC_debug', Image, queue_size=10)
 
         self.reset_service = rospy.Service(TOPIC_PREFIX + NODE_NAME + '/reset', Empty, self.reset)
@@ -50,7 +54,7 @@ def get_vector(image, bins=32):
         histograms = [hist / hist.sum() for hist in histograms]  # Normalize histograms
 
         vector = np.concatenate(histograms, axis=0).reshape(-1)  # Create colour histogram vector
-        return vector
+        return vector.tolist()
 
     @staticmethod
     def euclidean(a, b):
@@ -109,20 +113,24 @@ def callback(self, data):
             return
         if nr_persons < 1:
             return
-
-        match, idx_match = self.compare_hoc(detected_persons)
-        if match:
-            msg = ColourCheckedTarget()
-            msg.time = time
-            msg.batch_nr = int(nr_batch)
-            msg.idx_person = int(idx_match)
-            msg.x_position = x_positions[idx_match]
-            msg.y_position = y_positions[idx_match]
-            msg.z_position = z_positions[idx_match]
-            msg.detected_person = detected_persons[idx_match]
-
-            self.publisher.publish(msg)
-            self.last_batch_processed = nr_batch
+        bridge = CvBridge()
+        # match, idx_match = self.compare_hoc(detected_persons)
+        colour_vectors = [self.get_vector(bridge.imgmsg_to_cv2(person, desired_encoding='passthrough')) for person in
+                          detected_persons]
+        # if match:
+
+        msg = ColourTarget()
+        msg.time = time
+        msg.nr_batch = nr_batch
+        msg.nr_persons = nr_persons
+        msg.x_positions = x_positions
+        msg.y_positions = y_positions
+        msg.z_positions = z_positions
+        msg.colour_vectors = [item for sublist in colour_vectors for item in sublist]
+        # msg.detected_person = detected_persons[idx_match]
+
+        self.publisher.publish(msg)
+        self.last_batch_processed = nr_batch
 
         # if nr_persons > 0 and match:
         #     self.publisher_debug.publish(detected_persons[idx_match])
diff --git a/people_tracking/src/people_tracker.py b/people_tracking/src/people_tracker.py
index dd9ffbe..6e32264 100755
--- a/people_tracking/src/people_tracker.py
+++ b/people_tracking/src/people_tracker.py
@@ -12,7 +12,7 @@
 
 # MSGS
 from sensor_msgs.msg import Image
-from people_tracking.msg import ColourCheckedTarget
+from people_tracking.msg import ColourCheckedTarget, ColourTarget
 from people_tracking.msg import DetectedPerson
 
 # SrvS
@@ -27,6 +27,10 @@
 CAMERA_WIDTH = 640
 CAMERA_HEIGHT = 480
 
+from collections import namedtuple
+
+Detection = namedtuple("Detection", ["batch_nr", "idx_person", "time", "x", "y", "z"])
+Persons = namedtuple("Persons", ["nr_batch", "time", "nr_persons", "x_positions", "y_positions", "z_positions", "colour_vectors", "face_detected"])
 
 
 class PeopleTracker:
@@ -34,18 +38,18 @@ def __init__(self) -> None:
 
         # ROS Initialize
         rospy.init_node(NODE_NAME, anonymous=True)
-        self.subscriber_HoC = rospy.Subscriber(TOPIC_PREFIX + 'HoC', ColourCheckedTarget,
-                                               self.callback_hoc, queue_size=5)
-        self.subscriber_Face = rospy.Subscriber(TOPIC_PREFIX + 'face_detections', ColourCheckedTarget,
+        self.subscriber_hoc = rospy.Subscriber(TOPIC_PREFIX + 'HoC', ColourTarget, self.callback_hoc,
+                                               queue_size=5)
+        self.subscriber_face = rospy.Subscriber(TOPIC_PREFIX + 'face_detections', ColourCheckedTarget,
                                                 self.callback_face, queue_size=5)
         self.subscriber_persons = rospy.Subscriber(TOPIC_PREFIX + 'person_detections', DetectedPerson,
                                                    self.callback_persons, queue_size=5)
-        self.subscriber_frames = rospy.Subscriber(name_subscriber_RGB, Image, self.get_latest_image, queue_size=1)
+        self.subscriber_image_raw = rospy.Subscriber(name_subscriber_RGB, Image, self.get_latest_image, queue_size=1)
 
         self.publisher_debug = rospy.Publisher(TOPIC_PREFIX + 'debug/people_tracker', Image, queue_size=10)
         self.rate = rospy.Rate(20)  # 20hz
 
-        # Create a ROS Service Proxy for the color histogram reset service
+        # Create a ROS Service Proxys for the reset services
         rospy.wait_for_service(TOPIC_PREFIX + 'HoC/reset')
         self.hoc_reset_proxy = rospy.ServiceProxy(TOPIC_PREFIX + 'HoC/reset', Empty)
         rospy.wait_for_service(TOPIC_PREFIX + 'Face/reset')
@@ -54,32 +58,54 @@ def __init__(self) -> None:
         self.detection_reset_proxy = rospy.ServiceProxy(TOPIC_PREFIX + 'person_detection/reset', Empty)
 
         # Variables
-        self.hoc_detections = [] # latest detected persons with hoc
-        self.face_detections = []   # latest detected persons with face
-        self.new_reidentify_hoc = False
-        self.time_received_hoc = None
-        self.new_reidentify_face = False
-        self.time_received_face = None
+        self.hoc_detections = []  # latest detected persons with hoc
+        self.new_hoc_detection = False
+        self.time_hoc_detection_sec = None
+
+        self.face_detections = []  # latest detected persons with face
+        self.new_face_detection = False
+        self.time_face_detection_sec = None
 
-        self.detections = [] # All persons detected per frame
+        self.detections = []  # All persons detected per frame
+        self.new_detections = False
 
         self.latest_image = None
-        self.tracked_data = []  # Data put into the prediction UKF
 
+        self.ukf_hoc = UKF()
+        self.data_hoc = []
 
         self.ukf_face = UKF()
-        self.data_confirmed_face = [] # list of data added to confirmed and not to face
+        self.data_face = []  # list of data added to confirmed and not to face
+
+        self.ukf_data_association = UKF()   # UKF with the data association
+        self.data_data_association = []
 
         self.ukf_confirmed = UKF()
-        self.ukf_prediction = UKF()
 
     def reset(self):
         """ Reset all stored variables in Class to their default values."""
+        self.hoc_detections = []  # latest detected persons with hoc
+        self.new_hoc_detection = False
+        self.time_hoc_detection_sec = None
+
+        self.face_detections = []  # latest detected persons with face
+        self.new_face_detection = False
+        self.time_face_detection_sec = None
+
+        self.detections = []  # All persons detected per frame
+
         self.latest_image = None
-        self.tracked_data = []
+
+        self.ukf_hoc = UKF()
+        self.data_hoc = []
+
+        self.ukf_face = UKF()
+        self.data_face = []  # list of data added to confirmed and not to face
 
         self.ukf_confirmed = UKF()
-        self.ukf_prediction = UKF()
+
+        self.ukf_data_association = UKF()
+        self.data_data_association = []
 
     @staticmethod
     def euclidean_distance(point1, point2):
@@ -89,12 +115,13 @@ def euclidean_distance(point1, point2):
         :param point2: A tuple or list representing the coordinates of the second point.
         :return: The Euclidean distance between the two points.
         """
+
         if len(point1) != len(point2):
             raise ValueError("Not the same dimensions")
 
-        squared_sum = sum((coord2 - coord1) ** 2 for coord1, coord2 in zip(point1, point2))
-        distance = math.sqrt(squared_sum)
-        return distance
+        a = np.array(point1)
+        b = np.array(point2)
+        return np.linalg.norm(a - b)
 
     @staticmethod
     def element_exists(lst, element):
@@ -110,23 +137,54 @@ def element_exists(lst, element):
         except ValueError:  # If element is not in the list
             return False, None
 
-    def callback_hoc(self, data):
-        """ Add the latest HoC detection to the storage."""
-        time = data.time
-        batch_nr = data.batch_nr
-        idx_person = data.idx_person
-        x_position = data.x_position
-        y_position = data.y_position
-        z_position = data.z_position
+    def reset_color_histogram_node(self):
+        """ Call the color histogram reset service."""
 
-        if len(self.hoc_detections) >= 5:
-            self.hoc_detections = self.hoc_detections[1:]
-        self.hoc_detections.append([batch_nr, idx_person, time, x_position, y_position, z_position])
+        try:
+            response = self.hoc_reset_proxy()
+            rospy.loginfo("Color histogram node reset successfully.")
+        except rospy.ServiceException as e:
+            rospy.logerr("Failed to reset color histogram node: %s", str(e))
 
-        self.new_reidentify_hoc = True
-        self.time_received_hoc = float(rospy.get_time())
+    def reset_face_node(self):
+        """ Call the face reset service."""
+        try:
+            response = self.face_reset_proxy()
+            rospy.loginfo("Face node reset successfully.")
+        except rospy.ServiceException as e:
+            rospy.logerr("Failed to reset Face node: %s", str(e))
 
-    def callback_face(self, data):
+    def reset_detection(self):
+        """ Call the people_detection reset service."""
+        try:
+            response = self.detection_reset_proxy()
+            rospy.loginfo("Detection node reset successfully.")
+        except rospy.ServiceException as e:
+            rospy.logerr("Failed to reset detection node: %s", str(e))
+
+    def get_latest_image(self, data: Image) -> None:
+        """ Get the most recent frame/image from the camera."""
+        self.latest_image = data
+
+    def callback_hoc(self, data: ColourCheckedTarget, amount_detections_stored: int = 100) -> None:
+        """ Add the latest HoC detection to the storage."""
+        time = data.time
+        batch_nr = data.nr_batch
+        idx_person = data.nr_persons
+        x_position = data.x_positions
+        y_position = data.y_positions
+        z_position = data.z_positions
+        colour_vectors = [data.colour_vectors[i:i + 32*3] for i in range(0, len(data.colour_vectors), 32*3)]
+
+        # rospy.loginfo(colour_vectors)
+        # if len(self.hoc_detections) >= amount_detections_stored:
+        #     self.hoc_detections.pop(0)
+        # self.hoc_detections.append(Detection(batch_nr, idx_person, time, x_position, y_position, z_position))
+        #
+        # self.new_hoc_detection = True
+        # self.time_hoc_detection_sec = float(rospy.get_time())
+
+    def callback_face(self, data: ColourCheckedTarget, amount_detections_stored: int = 100) -> None:
         """ Add the latest Face detection to the storage."""
         time = data.time
         batch_nr = data.batch_nr
@@ -135,15 +193,15 @@ def callback_face(self, data):
         y_position = data.y_position
         z_position = data.z_position
 
-        if len(self.face_detections) >= 5:
-            self.face_detections = self.face_detections[1:]
-        self.face_detections.append([batch_nr, idx_person, time, x_position, y_position, z_position])
+        if len(self.face_detections) >= amount_detections_stored:
+            self.face_detections.pop(0)
+        self.face_detections.append(Detection(batch_nr, idx_person, time, x_position, y_position, z_position))
 
-        self.new_reidentify_face = True
-        self.time_received_face = float(rospy.get_time())
+        self.new_face_detection = True
+        self.time_face_detection_sec = float(rospy.get_time())
 
-    def callback_persons(self, data) -> None:
-        """ Update the ukf_prediction using the closest image. (Data Association based on distance)."""
+    def callback_persons(self, data: DetectedPerson) -> None:
+        """ Add the latest detected persons from people_detection to the storage."""
         time = data.time
         nr_batch = data.nr_batch
         nr_persons = data.nr_persons
@@ -151,32 +209,35 @@ def callback_persons(self, data) -> None:
         y_positions = data.y_positions
         z_positions = data.z_positions
 
-        self.detections.append([nr_batch, time, nr_persons, x_positions, y_positions, z_positions])
-        self.data_association(self.detections[-1]) #this one could case the to far ahead  data in reindentifier?
+        if len(self.detections) >= 100:
+            self.detections.pop(0)
+        self.detections.append(Persons(nr_batch, time, nr_persons, x_positions, y_positions, z_positions))
+        self.new_detections = True
+        # rospy.loginfo([person.nr_batch for person in self.detections])
 
-    def data_association(self, detection:  List[Union[int, float, int, List[int], List[int], List[int]]]):
-        """ Perform data association based on euclidean distance between latest and the input. Also updates the
+    def do_data_association(self, detection: Persons) -> None:
+        """ Perform data association based on Euclidean distance between latest and the input. Also updates the
         latest measurement.
 
         :param detection: [nr_batch, time, nr_persons, x_positions, y_positions, z_positions]
         """
         nr_batch, time, nr_persons, x_positions, y_positions, z_positions = detection
 
-        if len(self.tracked_data) < 1:  # Check if it is the first detection
-            self.tracked_data.append(
-                [nr_batch, 0, time, x_positions[0], y_positions[0], z_positions[0]])
+        if nr_persons <= 0:  # Return if there are no persons in detection
             return
 
-        if time < self.tracked_data[-1][0]:     # Return if the detection is "old"
+        if len(self.data_data_association) < 1:  # Check if it is the first detection, if so select first target
+            self.data_data_association.append(
+                [nr_batch, 0, time, x_positions[0], y_positions[0], z_positions[0]])
             return
 
-        if nr_persons <= 0:  # Return if there are no persons in detection
+        if time < self.data_data_association[-1][0]:  # Return if the detection is "old"
             return
 
         smallest_distance = None
         person = None
         for idx in range(nr_persons):
-            tracked = tuple(self.tracked_data[-1][-3:])
+            tracked = tuple(self.data_data_association[-1][-3:])
             distance = self.euclidean_distance(tracked,
                                                tuple([x_positions[idx], y_positions[idx], z_positions[idx]]))
             if smallest_distance is None:
@@ -186,53 +247,51 @@ def data_association(self, detection:  List[Union[int, float, int, List[int], Li
                 person = idx
                 smallest_distance = distance
 
-        self.tracked_data.append(
-            [nr_batch, person, time, x_positions[person], y_positions[person], z_positions[person]])
-        self.ukf_prediction.update(time, [x_positions[person], y_positions[person], 0])  # ToDo move make sure to add time stamp check?
+        self.data_data_association.append(
+            Detection(nr_batch, person, time, x_positions[person], y_positions[person], z_positions[person]))
+        if self.ukf_data_association.current_time < time:
+            self.ukf_data_association.update(time, [x_positions[person], y_positions[person], 0])
 
-    def redo_data_association(self, start_batch, idx_person):
+    def redo_data_association(self, start_batch: int, idx_person: int):
         """ Redo the data association from a start batch nr.
         Assumption:
         * The start batch number is in the stored data
         """
-        batch_numbers = [detection[0] for detection in self.detections]
-        rospy.loginfo("batch_nr: %s, start_batch:%s ", batch_numbers, start_batch)
+        batch_numbers = [detection.nr_batch for detection in self.detections]
+        # rospy.loginfo("batch_nr: %s, start_batch:%s ", batch_numbers, start_batch)
 
         exists, idx = self.element_exists(batch_numbers, start_batch)
 
         if not exists:
-            rospy.loginfo("Not Possible to redo data association")
+            rospy.logerr("Not Possible to redo data association")
             return
 
-        self.ukf_prediction = copy.deepcopy(self.ukf_confirmed)
+        self.ukf_data_association = copy.deepcopy(self.ukf_confirmed)
 
         nr_batch, time, nr_persons, x_positions, y_positions, z_positions = self.detections[idx]
 
-        self.tracked_data = [[nr_batch, time, idx_person, x_positions[idx_person], y_positions[idx_person], z_positions[idx_person]]]
+        self.data_data_association = [Detection(nr_batch, time, idx_person, x_positions[idx_person], y_positions[idx_person],
+                                          z_positions[idx_person])]
 
-        for detection in self.detections[idx+1:]:
-            self.data_association(detection)
+        for detection in self.detections[idx + 1:]:
+            self.do_data_association(detection)
         rospy.loginfo("Redone data association")
 
-    def get_latest_image(self, data):
-        """ Get the most recent frame/image from the camera."""
-        self.latest_image = data
-
-    def plot_tracker(self):
+    def plot_tracker(self): #TODO Good enough?
         """ Plot the trackers on a camera frame and publish it.
         This can be used to visualise all the output from the trackers. [x,y coords] Currently not for depth
         """
-        if len(self.tracked_data) >= 1:
+        if len(self.data_data_association) >= 1:
             latest_image = self.latest_image
             bridge = CvBridge()
             cv_image = bridge.imgmsg_to_cv2(latest_image, desired_encoding='passthrough')
 
             current_time = float(rospy.get_time())
-            if self.ukf_prediction.current_time < current_time:
-                ukf_predict = copy.deepcopy(self.ukf_prediction)
+            if self.ukf_data_association.current_time < current_time:  # Get prediction for current time
+                ukf_predict = copy.deepcopy(self.ukf_data_association)
                 ukf_predict.predict(current_time)
             else:
-                ukf_predict = self.ukf_prediction
+                ukf_predict = self.ukf_data_association
 
             x_hoc = int(self.ukf_confirmed.kf.x[0])
             y_hoc = int(self.ukf_confirmed.kf.x[2])
@@ -247,152 +306,128 @@ def plot_tracker(self):
             cv2.circle(cv_image, (x_hoc, y_hoc), 5, (255, 0, 0, 50), -1)  # plot latest hoc measurement blue
             cv2.circle(cv_image, (x_position, y_position), 5, (0, 0, 255, 50),
                        -1)  # plot ukf prediction measurement red
-            cv2.circle(cv_image, (self.tracked_data[-1][-3], self.tracked_data[-1][-2]), 5, (0, 255, 0, 20),
+            cv2.circle(cv_image, (self.data_data_association[-1][-3], self.data_data_association[-1][-2]), 5, (0, 255, 0, 20),
                        -1)  # plot latest data ass. measurement green
 
             tracker_image = bridge.cv2_to_imgmsg(cv_image, encoding="passthrough")
 
             # rospy.loginfo("predict x: %s, y: %s; measured x: %s, y: %s, HoC x: %s, y: %s", x_position, y_position,
-            #               self.tracked_data[-1][-3], self.tracked_data[-1][-2], x_hoc, y_hoc)
+            #               self.data_data_association[-1][-3], self.data_data_association[-1][-2], x_hoc, y_hoc)
 
             self.publisher_debug.publish(tracker_image)
 
-
-    def update_confirmed_tracker(self, update_idx, type):
+    def update_ukf_tracker(self, ukf: UKF, ukf_update_data, ukf_data, time):   #TODO Good enough?
         """ Update the known UKF up until given index."""
 
-        if type == "hoc":
-            self.data_confirmed_face.append(self.tracked_data[:update_idx])
-
-            update_data = self.tracked_data[:update_idx + 1][:]
-            for entry in update_data:
-                z = [entry[3], entry[4], entry[5]]
-                self.ukf_confirmed.update(entry[2], z)
-            self.tracked_data = self.tracked_data[update_idx:][:]
+        if ukf.current_time < time:
+            for entry in ukf_update_data:
+                z = [entry.x, entry.y, entry.z]
+                ukf.update(entry.time, z)
 
-        if type == "face":
-            rospy.loginfo("face update not done")
+            ukf_data.append(ukf_update_data)
 
-
-        # rospy.loginfo("Confirmed Update")
-
-    def reidentify_target(self):
+    def reidentify_target(self):    #TODO Good enough?
         """ Check if the target that is being followed is still the correct one."""
-        # rospy.loginfo("H: %s, F: %s", self.new_reidentify_hoc, self.new_reidentify_face)
-        if not self.new_reidentify_face and not self.new_reidentify_hoc:    # No new re-identification features found
+        # rospy.loginfo("H: %s, F: %s", self.new_hoc_detection, self.new_face_detection)
+        if not self.new_face_detection and not self.new_hoc_detection:  # No new re-identification features found
             rospy.loginfo("None")
             current_time = float(rospy.get_time())
-            if self.time_received_hoc is None:  # Check that there is a measurement
+
+            times = [self.time_hoc_detection_sec, self.time_face_detection_sec]
+            max_index, max_time = max(enumerate(times), key=lambda x: x[1] if x[1] is not None else float('-inf'))  # Find which identifier has been updated last
+
+            if max_time is None:   # In case there are no detections
                 return
-            if current_time - self.time_received_hoc > 5: # ToDo still determine correct time for this
-                if self.tracked_data[-1][-3] > CAMERA_WIDTH - 50 or self.tracked_data[-1][-3] < 50:
+
+            if current_time - max_time > 5:
+                values = [self.hoc_detections[-1], self.face_detections[-1]]
+                if values[max_index].x > CAMERA_WIDTH - 50 or values[max_index].x < 50:
                     rospy.loginfo("Target Lost: Out Of Frame")
                 else:
-                    rospy.loginfo("Target Lost: No re-identifier found for a while")
-                    # DA + set new Hoc from this? --> Look into best option
+                    rospy.loginfo("Target Lost: No re-identifier found")
             return
 
-        if self.new_reidentify_face and self.new_reidentify_hoc: # both a face and HoC update
-            face_exists, face_idx = self.element_exists(self.tracked_data, self.face_detections[-1])
-            hoc_exists, hoc_idx = self.element_exists(self.tracked_data, self.hoc_detections[-1])
-
-            if face_exists and hoc_exists: # both face and hoc in same DA line, so assume still on correct target
-               rospy.loginfo("Both")
-               # Update UKF and prune data up until newest
-               if self.face_detections[-1][0] > self.hoc_detections[-1][0]:
-                    self.update_confirmed_tracker(face_idx, "face")
-                    if len(self.detections) > 10:
-                        self.detections = self.detections[-10:]
-               else:
-                   self.update_confirmed_tracker(hoc_idx, "hoc")
-
-            else:   # if gone potentially to the wrong path
-                self.ukf_prediction = copy.deepcopy(self.ukf_confirmed)
-                # Reset tracked_data with the latest detection (either face or hoc)
-                if face_exists: # if face in there -> Update UKF and prune till face, reset hoc to face data detection
-                    rospy.loginfo("Both - Face")
-                    self.update_confirmed_tracker(face_idx, "face")
-                    if len(self.detections) > 10:
-                        self.detections = self.detections[-10:]
-                    self.redo_data_association(self.face_detections[-1][0], self.face_detections[-1][2])
-                    #  TODO reset hoc to face detection point
-                elif hoc_exists:
-                    rospy.loginfo("Both-HoC")
-                    # TODO if face not in there but HoC in there -> go to back to last known and check in all data from face measurement
-                    # and continue from there with DA and check if hoc reapears (if hoc is newwr) if not, re-set and update hoc from old data from new point with da
-            self.new_reidentify_face = False
-            self.new_reidentify_hoc = False
+
+        if self.new_face_detection and self.new_hoc_detection:  # both a face and HoC update
+            rospy.loginfo("Face and HoC")
+            self.new_face_detection = False
+            self.new_hoc_detection = False
             return
 
-        if self.new_reidentify_face:   # Only face update
-            rospy.loginfo("Only Face")
-            #make sure that this is still in line with hoc ?
-            face_exists, face_idx = self.element_exists(self.tracked_data, self.face_detections[-1])
-            if face_exists:
-                self.update_confirmed_tracker(face_idx, "face")
-                if len(self.detections) > 10:
-                    self.detections = self.detections[-10:]
-            else:
-                self.redo_data_association(self.face_detections[-1][0], self.face_detections[-1][1])
 
-            self.new_reidentify_face = False
-            return
 
-        if self.new_reidentify_hoc:    # Only Hoc Update
-            rospy.loginfo("Only HoC")
-            # Make sure that this is still in line with face ?
-            hoc_exists, hoc_idx = self.element_exists(self.tracked_data, self.hoc_detections[-1])
-            if not hoc_exists:
-                if self.hoc_detections[-1][0] >= self.tracked_data[-1][0]:
-                    self.new_reidentify_hoc = True
-                else:
-                    self.redo_data_association(self.hoc_detections[-1][0], self.hoc_detections[-1][1])
-            else:
-                self.update_confirmed_tracker(hoc_idx, "hoc")
+        if self.new_face_detection:  # Only face update
+            face_detection = self.face_detections[-1]
+
+            try:    # If there is a hoc_detection work with this
+                hoc_detection = self.hoc_detections[-1]
+            except IndexError:
+                hoc_detection = Detection(0, 0, 0, 0, 0, 0)
+                rospy.loginfo("Only Face - IndexError")
 
-            self.new_reidentify_hoc = False
+            if face_detection.time < hoc_detection.time:    # If HoC is further along than face measurements
+                rospy.loginfo("Only Face - face time < hoc time")
+            else:   # Face newer than recent HoC measurement
+                rospy.loginfo("Only Face - face time >= hoc time")
+
+                # Check DA for face
+                exists, idx = self.element_exists(self.data_data_association, face_detection)
+                if exists:  # Update the face UKF
+                    self.update_ukf_tracker(self.ukf_face, self.data_data_association[:idx+1], self.data_face, face_detection.time)
+                else:   # Redo the data association
+                    self.redo_data_association(face_detection.batch_nr, face_detection.idx_person)
+
+            self.new_face_detection = False
             return
 
 
+        if self.new_hoc_detection:  # Only Hoc Update
+            hoc_detection = self.hoc_detections[-1]
+
+            try:    # If there is a hoc_detection work with this
+                face_detection = self.face_detections[-1]
+            except IndexError:
+                face_detection = Detection(0, 0, 0, 0, 0, 0)
+                rospy.loginfo("Only Hoc - IndexError")
 
+            if hoc_detection.time < face_detection.time:    # If Face is further along than Hoc measurements
+                rospy.loginfo("Only HoC - HoC time < face time")
+            else:   # HoC newer than recent Face measurement
+                rospy.loginfo("Only HoC - Hoc time >= face time")
 
+                # Check DA for HoC
+                exists, idx = self.element_exists(self.data_data_association, hoc_detection)
+                if exists:  # Update the HoC UKF
+                    self.update_ukf_tracker(self.ukf_hoc, self.data_data_association[:idx+1], self.data_hoc, hoc_detection.time)
+                else:   # Redo the data association
+                    self.redo_data_association(hoc_detection.batch_nr, hoc_detection.idx_person)
+
+            self.new_hoc_detection = False
+            return
+
+        rospy.loginfo("Only HoC")
+        self.new_hoc_detection = False
+        return
+        # if self.ukf_hoc.current_time > self.ukf_confirmed.current_time and self.ukf_face.current_time > self.ukf_confirmed.current_time:    # if both trackers further than confirmed tracker
+            # update confirmed tracker with data form the "oldest tracker"
+            # -> How to make sure that both UKFs are consistent?
 
 
 
 
     def loop(self):
-        """ Loop that repeats itself at self.rate.
-            Currently used for publishing the tracker data on an image.
-        """
+        """ Loop that repeats itself at self.rate. Can be used to execute methods at given rate. """
         while not rospy.is_shutdown():
+            if self.new_detections:  # Do data association with most recent detection.
+                self.do_data_association(self.detections[-1])
+
             if self.latest_image is not None:
                 self.plot_tracker()
 
-            self.reidentify_target() # run the re-identify function at the given frequency
+            # self.reidentify_target()  # run the re-identify function at the given frequency
 
-            # if self.latest_image:
-            #     self.reset_color_histogram_node()
             self.rate.sleep()
 
-    def reset_color_histogram_node(self):
-
-        # Call the color histogram reset service
-        try:
-            response = self.hoc_reset_proxy()
-            rospy.loginfo("Color histogram node reset successfully.")
-        except rospy.ServiceException as e:
-            rospy.logerr("Failed to reset color histogram node: %s", str(e))
-        try:
-            response = self.face_reset_proxy()
-            rospy.loginfo("Face node reset successfully.")
-        except rospy.ServiceException as e:
-            rospy.logerr("Failed to reset Face node: %s", str(e))
-        try:
-            response = self.detection_reset_proxy()
-            rospy.loginfo("Detection node reset successfully.")
-        except rospy.ServiceException as e:
-            rospy.logerr("Failed to reset detection node: %s", str(e))
-
 
 if __name__ == '__main__':
     try: