diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index a4d10991a4fd0b8705c6dfa8f1cf8343e11b69b7..e8abd88fd24066c754efa2682f16786462b989b4 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -318,13 +318,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
         }
 
         // here ends: data fetching
-        
-        humanTracker.update(HumanTracker::Measurements{
-            .humanPoses = humanPoseResult.humanPoses
-        });
-
-
 
+        humanTracker.update(HumanTracker::Measurements{.detectionTime = timestamp,
+                                                       .humanPoses = humanPoseResult.humanPoses});
     }
 
 
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 61eaaa69ff8b6f59e2488988ba983db5127aeb12..a63f8a30c6dcf00575f2e8252605a008db584468 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -16,7 +16,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
         const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
         ARMARX_CHECK_NOT_EMPTY(keypoints);
 
-        auto centerPos = core::Position::Zero();
+        Eigen::Vector3f centerPos;
         int size = 0;
         for (const auto& [_, v] : keypoints)
         {
@@ -29,7 +29,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
         centerPos /= size;
 
         core::Pose2D pose = core::Pose2D::Identity();
-        pose.translation() = centerPos;
+        pose.translation() = centerPos.head(2);
         //TODO: angle
         pose.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
 
@@ -69,33 +69,33 @@ namespace armarx::navigation::components::dynamic_scene_provider
 
     struct PosDistance
     {
-        HumanTracker::TrackedHuman& oldHuman;
-        HumanTracker::DetectedHuman& newHuman;
-        const float distance;
+        HumanTracker::TrackedHuman* oldHuman;
+        HumanTracker::DetectedHuman* newHuman;
+        float distance;
     };
 
     std::vector<PosDistance>
-    getSortedDistances(const std::vector<HumanTracker::TrackedHuman>& oldHumans,
-                       const std::vector<HumanTracker::DetectedHuman>& newHumans)
+    getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
+                       std::vector<HumanTracker::DetectedHuman>& newHumans)
     {
         std::vector<PosDistance> posDistances;
 
-        for (const auto& oldHuman : oldHumans)
+        for (auto& oldHuman : oldHumans)
         {
             if (oldHuman.associated)
             {
                 continue;
             }
-            for (const auto& newHuman : newHumans)
+            for (auto& newHuman : newHumans)
             {
                 if (newHuman.associated)
                 {
                     continue;
                 }
-                posDistances.emplace_back(
-                    oldHuman,
-                    newHuman,
-                    (newHuman.pose.translation() - oldHuman.human.pose.translation()).norm());
+                posDistances.push_back(
+                    {&oldHuman,
+                     &newHuman,
+                     (newHuman.pose.translation() - oldHuman.human.pose.translation()).norm()});
             }
         }
 
@@ -125,7 +125,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
                 }
                 if (oldHuman.trackingId.value() == newHuman.trackingId.value())
                 {
-                    associate(oldHuman, newHuman);
+                    associate(&oldHuman, &newHuman);
                 }
             }
         }
@@ -143,7 +143,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
             {
                 break;
             }
-            if (posDistance.oldHuman.associated || posDistance.newHuman.associated)
+            if (posDistance.oldHuman->associated || posDistance.newHuman->associated)
             {
                 continue;
             }
@@ -152,27 +152,27 @@ namespace armarx::navigation::components::dynamic_scene_provider
     }
 
     void
-    HumanTracker::associate(TrackedHuman& trackedHuman, DetectedHuman& detectedHuman)
+    HumanTracker::associate(TrackedHuman* trackedHuman, DetectedHuman* detectedHuman)
     {
-        ARMARX_CHECK(!trackedHuman.associated);
-        ARMARX_CHECK(!detectedHuman.associated);
+        ARMARX_CHECK(!trackedHuman->associated);
+        ARMARX_CHECK(!detectedHuman->associated);
 
-        trackedHuman.associated = true;
-        detectedHuman.associated = true;
+        trackedHuman->associated = true;
+        detectedHuman->associated = true;
 
         // TODO alpha parameter
         float a = 0.7;
 
         float dt =
-            (detectedHuman.detectionTime - trackedHuman.human.detectionTime).toSecondsDouble();
+            (detectedHuman->detectionTime - trackedHuman->human.detectionTime).toSecondsDouble();
         Eigen::Vector2f ds =
-            (detectedHuman.pose.translation() - trackedHuman.human.pose.translation());
+            (detectedHuman->pose.translation() - trackedHuman->human.pose.translation());
         Eigen::Vector2f linVelocity = ds / dt;
 
-        Eigen::Vector2f velocity = a * linVelocity + (1 - a) * trackedHuman.human.linearVelocity;
+        Eigen::Vector2f velocity = a * linVelocity + (1 - a) * trackedHuman->human.linearVelocity;
 
-        trackedHuman.human = {detectedHuman.pose, velocity, detectedHuman.detectionTime};
-        trackedHuman.trackingId = detectedHuman.trackingId;
+        trackedHuman->human = {detectedHuman->pose, velocity, detectedHuman->detectionTime};
+        trackedHuman->trackingId = detectedHuman->trackingId;
     }
 
     std::vector<human::Human>
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index f13a6653390485c57c881766899c423e07271a20..e5197d8222457b91992bc2c74b89a65c4a704fb0 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -24,9 +24,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
 
         struct DetectedHuman
         {
-            const core::Pose2D pose;
-            const std::optional<std::string> trackingId;
-            const DateTime detectionTime;
+            core::Pose2D pose;
+            std::optional<std::string> trackingId;
+            DateTime detectionTime;
             bool associated;
         };
 
@@ -45,7 +45,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
 
     private:
         void associateHumans(std::vector<DetectedHuman>& detectedHumans);
-        void associate(TrackedHuman& tracked, DetectedHuman& detected);
+        void associate(TrackedHuman* tracked, DetectedHuman* detected);
 
     private:
         std::vector<TrackedHuman> trackedHumans;