diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp
index 0d9f3acb8d66..85c274ab5d5c 100644
--- a/src/lib/motion_planning/PositionSmoothing.cpp
+++ b/src/lib/motion_planning/PositionSmoothing.cpp
@@ -106,7 +106,10 @@ float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const
 
 float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
 {
-	const Vector3f &start_position = waypoints[0];
+	const Vector3f &start_position = {_trajectory[0].getCurrentPosition(),
+					  _trajectory[1].getCurrentPosition(),
+					  _trajectory[2].getCurrentPosition()
+					 };
 	const Vector3f &target = waypoints[1];
 	const Vector3f &next_target = waypoints[2];
 
@@ -114,10 +117,6 @@ float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
 	const Vector2f target_xy_z = {target.xy().norm(), target(2)};
 	const Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)};
 
-	Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
-			  _trajectory[1].getCurrentPosition(),
-			  _trajectory[2].getCurrentPosition());
-
 	float arrival_z_speed = 0.0f;
 	const bool target_next_different = fabsf(target(2) - next_target(2)) > 0.001f;
 
@@ -132,7 +131,7 @@ float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
 		arrival_z_speed = math::min(max_speed_in_turn, _trajectory[2].getMaxVel());
 	}
 
-	const float distance_start_target = fabs(target(2) - pos_traj(2));
+	const float distance_start_target = fabs(target(2) - start_position(2));
 	float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance(
 					    _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(),
 					    distance_start_target, arrival_z_speed));