diff --git a/people_msgs/msg/Person.msg b/people_msgs/msg/Person.msg
index c3a4a522..e0398f99 100644
--- a/people_msgs/msg/Person.msg
+++ b/people_msgs/msg/Person.msg
@@ -4,4 +4,4 @@ geometry_msgs/Point velocity
 float64             reliability
 string[]            tagnames
 string[]            tags
-
+people_msgs/PositionMeasurement position_measurement
diff --git a/people_velocity_tracker/scripts/tracker.py b/people_velocity_tracker/scripts/tracker.py
index a76e5ebe..05f097f9 100755
--- a/people_velocity_tracker/scripts/tracker.py
+++ b/people_velocity_tracker/scripts/tracker.py
@@ -93,6 +93,7 @@ def get_person(self):
         p.position = self.pos.pos
         p.velocity = self.velocity()
         p.reliability = self.reliability
+        p.position_measurement = self.pos
         return self.pos.header.frame_id, p
 
 
@@ -123,9 +124,9 @@ def spin(self):
         while not rospy.is_shutdown():
             # Remove People Older Than timeout param
             now = rospy.Time.now()
-            for p in self.people.values():
+            for p in list(self.people.values()):
                 if now - p.age() > self.TIMEOUT:
-                    del self.people[p.id()]
+                    del self.people[p.get_id()]
             self.publish()
             rate.sleep()
 
@@ -134,7 +135,7 @@ def publish(self):
         pl = People()
         pl.header.frame_id = None
 
-        for p in self.people.values():
+        for p in list(self.people.values()):
             p.publish_markers(self.mpub)
             frame, person = p.get_person()
             pl.header.frame_id = frame