diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp
index e38a98ed6b0..852db11529d 100644
--- a/nav2_mppi_controller/src/critics/goal_critic.cpp
+++ b/nav2_mppi_controller/src/critics/goal_critic.cpp
@@ -57,7 +57,7 @@ void GoalCritic::score(CriticData & data)
     xt::pow(traj_x - goal_x, 2) +
     xt::pow(traj_y - goal_y, 2));
 
-  data.costs += xt::pow(xt::mean(dists, {1}) * weight_, power_);
+  data.costs += xt::pow(xt::mean(dists, {1}, immediate) * weight_, power_);
 }
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
index aae95015e1b..6d0a3ceb6c1 100644
--- a/nav2_mppi_controller/src/optimizer.cpp
+++ b/nav2_mppi_controller/src/optimizer.cpp
@@ -378,8 +378,10 @@ void Optimizer::updateControlSequence()
   auto && softmaxes_extened = xt::eval(xt::view(softmaxes, xt::all(), xt::newaxis()));
 
   xt::noalias(control_sequence_.vx) = xt::sum(state_.cvx * softmaxes_extened, 0, immediate);
-  xt::noalias(control_sequence_.vy) = xt::sum(state_.cvy * softmaxes_extened, 0, immediate);
   xt::noalias(control_sequence_.wz) = xt::sum(state_.cwz * softmaxes_extened, 0, immediate);
+  if (isHolonomic()) {
+    xt::noalias(control_sequence_.vy) = xt::sum(state_.cvy * softmaxes_extened, 0, immediate);
+  }
 
   applyControlSequenceConstraints();
 }