diff --git a/nav2_frenet_ilqr_controller/src/costs/lateral_distance_cost.cpp b/nav2_frenet_ilqr_controller/src/costs/lateral_distance_cost.cpp
index 6978496..1fdf58f 100644
--- a/nav2_frenet_ilqr_controller/src/costs/lateral_distance_cost.cpp
+++ b/nav2_frenet_ilqr_controller/src/costs/lateral_distance_cost.cpp
@@ -37,7 +37,7 @@ double LateralDistanceCost::cost(
     trajectory_cost += std::abs(frenet_state[3]);
   }
 
-  return K_lateral_distance_ * trajectory_cost;
+  return K_lateral_distance_ * trajectory_cost / frenet_trajectory.size();
 }
 
 }
diff --git a/nav2_frenet_ilqr_controller/src/costs/longtitutal_velocity_cost.cpp b/nav2_frenet_ilqr_controller/src/costs/longtitutal_velocity_cost.cpp
index b1bc4e3..042f664 100644
--- a/nav2_frenet_ilqr_controller/src/costs/longtitutal_velocity_cost.cpp
+++ b/nav2_frenet_ilqr_controller/src/costs/longtitutal_velocity_cost.cpp
@@ -43,7 +43,7 @@ double LongtitutalVelocityCost::cost(
     trajectory_cost += std::abs(frenet_state[1] - desired_velocity_);
   }
 
-  return K_longtitutal_velocity_ * trajectory_cost;
+  return K_longtitutal_velocity_ * trajectory_cost / frenet_trajectory.size();
 }
 
 }
diff --git a/nav2_frenet_ilqr_controller/src/costs/obstacle_cost.cpp b/nav2_frenet_ilqr_controller/src/costs/obstacle_cost.cpp
index a2e51dc..bbbc7bd 100644
--- a/nav2_frenet_ilqr_controller/src/costs/obstacle_cost.cpp
+++ b/nav2_frenet_ilqr_controller/src/costs/obstacle_cost.cpp
@@ -45,7 +45,7 @@ double ObstacleCost::cost(
     trajectory_cost += point_cost;
   }
 
-  return K_obstacle_ * trajectory_cost;
+  return K_obstacle_ * trajectory_cost / cartesian_trajectory.size();
 }
 
 }