diff --git a/sim/src/components/Algorithm_Routecontrol/algorithm_RouteControl_implementation.cpp b/sim/src/components/Algorithm_Routecontrol/algorithm_RouteControl_implementation.cpp
index 866a38b05ea6192a798c4b4e964c81920360a109..dfbbc4867f2a6223a4475688dc8399eee95100b9 100644
--- a/sim/src/components/Algorithm_Routecontrol/algorithm_RouteControl_implementation.cpp
+++ b/sim/src/components/Algorithm_Routecontrol/algorithm_RouteControl_implementation.cpp
@@ -1,6 +1,6 @@
 /********************************************************************************
  * Copyright (c) 2020-2021 ITK Engineering GmbH
- *
+ *               2023 Hexad GmbH
  * This program and the accompanying materials are made available under the
  * terms of the Eclipse Public License 2.0 which is available at
  * http://www.eclipse.org/legal/epl-2.0.
@@ -259,7 +259,7 @@ void Algorithm_Routecontrol_Implementation::ReadWayPointData()
         if (i<n-1)
         {
             openScenario::TrajectoryPoint pointNext = trajectory.points.at(i+1);
-            vel = std::sqrt(std::pow(pointNext.x-point.x,2) + std::pow(pointNext.y-point.y,2)) / mTimeStep; // uniform motion approximation
+            vel = std::sqrt(std::pow(pointNext.x-point.x,2) + std::pow(pointNext.y-point.y,2)) / (pointNext.time-point.time); // uniform motion approximation
         }
         waypoints->at(i).longVelocity = vel;
     }