diff --git a/ilqr_trajectory_tracker/CMakeLists.txt b/ilqr_trajectory_tracker/CMakeLists.txt
index c95fb1c..e91b84a 100644
--- a/ilqr_trajectory_tracker/CMakeLists.txt
+++ b/ilqr_trajectory_tracker/CMakeLists.txt
@@ -16,7 +16,10 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 
-add_library(ilqr_trajectory_tracker_lib SHARED src/models/diff_robot_model.cpp)
+add_library(
+  ilqr_trajectory_tracker_lib SHARED 
+    src/models/diff_robot_model.cpp
+    src/models/ackermann_robot_model.cpp)
 
 target_include_directories(
   ilqr_trajectory_tracker_lib
diff --git a/ilqr_trajectory_tracker/demos/ilqr_optimizer_demo.cpp b/ilqr_trajectory_tracker/demos/ilqr_optimizer_demo.cpp
index 6ddb26d..0aca2c0 100644
--- a/ilqr_trajectory_tracker/demos/ilqr_optimizer_demo.cpp
+++ b/ilqr_trajectory_tracker/demos/ilqr_optimizer_demo.cpp
@@ -49,13 +49,12 @@ int main(int argc, char ** argv)
 
   Matrix3d Q = Matrix3d::Identity() * 10;
   Matrix2d R = Matrix2d::Identity() * 0.1;
-  double alpha = 1;
 
   ilqr_trajectory_tracker::NewtonOptimizer<DiffDriveRobotModel> newton_optimizer;
   newton_optimizer.setIterationNumber(10);
-  newton_optimizer.setAlpha(alpha);
 
   const auto start{std::chrono::steady_clock::now()};
+
   // TODO: Fix demo later, we should pass the actual robot pose instead of
   // the beginning pose of the feasible trajectory
   auto u_optimal = newton_optimizer.optimize(x_feasible[0], x_feasible, Q, R, dt);
diff --git a/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/ilqr_optimizer.hpp b/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/ilqr_optimizer.hpp
index 3ccd637..b8d1d88 100644
--- a/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/ilqr_optimizer.hpp
+++ b/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/ilqr_optimizer.hpp
@@ -19,8 +19,10 @@
 
 #include <Eigen/Dense>
 #include <cmath>
+#include <memory>
 #include <tuple>
 #include <algorithm>
+#include <limits>
 
 using namespace Eigen;
 
@@ -40,15 +42,19 @@ template<typename RobotModel>
 class NewtonOptimizer : public Optimizer
 {
 public:
-  NewtonOptimizer();
+  template<typename ... RobotModelParams>
+  NewtonOptimizer(const RobotModelParams ... model_params);
+
   std::vector<MatrixXd> backwardPass(
     const std::vector<typename RobotModel::StateT> & x_feasible,
     const std::vector<typename RobotModel::InputT> & u_feasible,
     const MatrixXd & Q, const MatrixXd & R, const double dt);
+
   std::tuple<MatrixXd, MatrixXd> solveDiscreteLQRProblem(
     const MatrixXd & A, const MatrixXd & B,
     const MatrixXd & Q, const MatrixXd & R,
     const MatrixXd & P);
+
   std::tuple<std::vector<typename RobotModel::StateT>,
     std::vector<typename RobotModel::InputT>> forwardPass(
     const typename RobotModel::StateT & x0,
@@ -60,23 +66,28 @@ class NewtonOptimizer : public Optimizer
     const typename RobotModel::StateT & x0,
     const std::vector<typename RobotModel::StateT> & x_feasible, const Matrix3d & Q,
     const Matrix2d & R, const double dt);
+
   double cost(
     const std::vector<typename RobotModel::StateT> & x_tracked,
     const std::vector<typename RobotModel::StateT> & x_trajectory);
 
   void setIterationNumber(const size_t iteration_number);
-  void setAlpha(const double alpha);
+
+  void setInputConstraints(
+    typename RobotModel::InputT input_limits_min,
+    typename RobotModel::InputT input_limits_max);
 
 private:
-  RobotModel robot_model_;
-  double alpha_;
+  std::unique_ptr<RobotModel> robot_model_;
   size_t iteration_number_;
 };
 
 template<typename RobotModel>
-NewtonOptimizer<RobotModel>::NewtonOptimizer()
+template<typename ... RobotModelParams>
+NewtonOptimizer<RobotModel>::NewtonOptimizer(const RobotModelParams ... model_params)
 : Optimizer()
 {
+  robot_model_ = std::make_unique<RobotModel>(model_params...);
 }
 
 template<typename RobotModel>
@@ -97,15 +108,15 @@ std::vector<MatrixXd> NewtonOptimizer<RobotModel>::backwardPass(
 
   for (auto i = std::distance(x_feasible.begin(), std::prev(x_feasible.end(), 2)); i >= 0; i--) {
     auto x_offset =
-      robot_model_.applySystemDynamics(x_feasible.at(i), u_feasible[i], dt) - x_feasible[i + 1];
+      robot_model_->applySystemDynamics(x_feasible.at(i), u_feasible[i], dt) - x_feasible[i + 1];
 
     MatrixXd A_tilda = MatrixXd::Identity(state_dimension + 1, state_dimension + 1);
-    auto A = robot_model_.getStateMatrix(x_feasible[i], u_feasible[i], dt);
+    auto A = robot_model_->getStateMatrix(x_feasible[i], u_feasible[i], dt);
     A_tilda.topLeftCorner(state_dimension, state_dimension) = A;
     A_tilda.topRightCorner(state_dimension, 1) = x_offset;
 
     MatrixXd B_tilda = MatrixXd::Zero(state_dimension + 1, input_dimension);
-    auto B = robot_model_.getControlMatrix(x_feasible[i], u_feasible[i], dt);
+    auto B = robot_model_->getControlMatrix(x_feasible[i], u_feasible[i], dt);
     B_tilda.topLeftCorner(state_dimension, input_dimension) = B;
 
     MatrixXd Q_tilda = MatrixXd::Identity(state_dimension + 1, state_dimension + 1);
@@ -137,16 +148,15 @@ std::tuple<std::vector<typename RobotModel::StateT>,
 
   // assert trajectory_size > 0
   x_tracked[0] = x0;
-  for (typename std::vector<typename RobotModel::StateT>::difference_type i = 0,
-    max_difference = std::distance(x_feasible.begin(), std::prev(x_feasible.end(), 1));
-    i < max_difference; ++i)
+  for (size_t i = 0; i < x_feasible.size() - 1; ++i)
   {
     auto x_error = x_tracked[i] - x_feasible[i];
     Vector<double, 4> z_error;
     z_error << x_error, alpha;
 
     u_applied[i] = u_feasible[i] + K_gains[i] * z_error;
-    x_tracked[i + 1] = robot_model_.applySystemDynamics(x_tracked[i], u_applied[i], dt);
+    u_applied[i] = robot_model_->applyLimits(u_applied[i]);
+    x_tracked[i + 1] = robot_model_->applySystemDynamics(x_tracked[i], u_applied[i], dt);
   }
 
   return {x_tracked, u_applied};
@@ -174,7 +184,7 @@ std::vector<typename RobotModel::InputT> NewtonOptimizer<RobotModel>::optimize(
 {
   // assert trajectory_size > 0
 
-  double alpha = alpha_;
+  double alpha = 1.0;
 
   std::vector<typename RobotModel::StateT> x_best_trajectory;
   std::vector<typename RobotModel::InputT> u_best_trajectory;
@@ -234,9 +244,11 @@ void NewtonOptimizer<RobotModel>::setIterationNumber(const size_t iteration_numb
 }
 
 template<typename RobotModel>
-void NewtonOptimizer<RobotModel>::setAlpha(const double alpha)
+void NewtonOptimizer<RobotModel>::setInputConstraints(
+  typename RobotModel::InputT input_limits_min,
+  typename RobotModel::InputT input_limits_max)
 {
-  alpha_ = alpha;
+  robot_model_->setLimits(input_limits_min, input_limits_max);
 }
 
 }
diff --git a/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/ackermann_robot_model.hpp b/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/ackermann_robot_model.hpp
new file mode 100644
index 0000000..1c8ad2a
--- /dev/null
+++ b/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/ackermann_robot_model.hpp
@@ -0,0 +1,47 @@
+// Copyright (C) 2024 Cihat Kurtuluş Altıparmak
+// Copyright (C) 2024 Prof. Tufan Kumbasar, Istanbul Technical University Artificial Intelligence and Intelligent Systems (AI2S) Laboratory
+// Copyright (C) 2024 Prof. Behçet Uğur Töreyin
+//
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+#pragma once
+
+#include <ilqr_trajectory_tracker/models/base_model.hpp>
+
+#include <Eigen/Dense>
+#include <cmath>
+
+using namespace Eigen;
+
+namespace ilqr_trajectory_tracker
+{
+
+using AckermannRobotModelState = Vector3d;
+using AckermannRobotModelInput = Vector2d;
+
+class AckermannRobotModel : public Model<AckermannRobotModelState, AckermannRobotModelInput>
+{
+public:
+  using StateT = AckermannRobotModelState;
+  using InputT = AckermannRobotModelInput;
+  AckermannRobotModel(const double wheelbase);
+  StateT applySystemDynamics(const StateT & x, const InputT & u, const double dt) override;
+  InputT applyLimits(const InputT & u) override;
+  MatrixXd getStateMatrix(const StateT & x_eq, const InputT & u_eq, const double dt);
+  MatrixXd getControlMatrix(const StateT & x_eq, const InputT & u_eq, const double dt);
+private:
+  double wheelbase_;
+};
+
+}
diff --git a/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/base_model.hpp b/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/base_model.hpp
index fbebb5d..914f19e 100644
--- a/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/base_model.hpp
+++ b/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/base_model.hpp
@@ -31,10 +31,15 @@ class Model
 public:
   Model();
   virtual StateT applySystemDynamics(const StateT & x, const InputT & u, const double dt) = 0;
+  virtual InputT applyLimits(const InputT & u) = 0;
+  void setLimits(const Vector2d & input_limits_min, const Vector2d & input_limits_max);
   virtual MatrixXd getStateMatrix(const StateT & x_eq, const InputT & u_eq, const double dt) = 0;
   virtual MatrixXd getControlMatrix(
     const StateT & x_eq, const InputT & u_eq,
     const double dt) = 0;
+protected:
+  InputT input_limits_min_;
+  InputT input_limits_max_;
 };
 
 template<typename StateT, typename InputT>
@@ -42,4 +47,12 @@ Model<StateT, InputT>::Model()
 {
 }
 
+template<typename StateT, typename InputT>
+void Model<StateT, InputT>::setLimits(
+  const Vector2d & input_limits_min,
+  const Vector2d & input_limits_max) {
+
+  input_limits_min_ = input_limits_min;
+  input_limits_max_ = input_limits_max;
+}
 }
diff --git a/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/diff_robot_model.hpp b/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/diff_robot_model.hpp
index 45ed62e..eef19bc 100644
--- a/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/diff_robot_model.hpp
+++ b/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/diff_robot_model.hpp
@@ -36,9 +36,10 @@ class DiffDriveRobotModel : public Model<DiffDriveRobotModelState, DiffDriveRobo
   using StateT = DiffDriveRobotModelState;
   using InputT = DiffDriveRobotModelInput;
   DiffDriveRobotModel();
-  Vector3d applySystemDynamics(const Vector3d & x, const Vector2d & u, const double dt) override;
-  MatrixXd getStateMatrix(const Vector3d & x_eq, const Vector2d & u_eq, const double dt);
-  MatrixXd getControlMatrix(const Vector3d & x_eq, const Vector2d & u_eq, const double dt);
+  DiffDriveRobotModelState applySystemDynamics(const StateT & x, const InputT & u, const double dt) override;
+  InputT applyLimits(const InputT & u) override;
+  MatrixXd getStateMatrix(const StateT & x_eq, const InputT & u_eq, const double dt);
+  MatrixXd getControlMatrix(const StateT & x_eq, const InputT & u_eq, const double dt);
 };
 
 }
diff --git a/ilqr_trajectory_tracker/src/models/ackermann_robot_model.cpp b/ilqr_trajectory_tracker/src/models/ackermann_robot_model.cpp
new file mode 100644
index 0000000..52ea52a
--- /dev/null
+++ b/ilqr_trajectory_tracker/src/models/ackermann_robot_model.cpp
@@ -0,0 +1,74 @@
+// Copyright (C) 2024 Cihat Kurtuluş Altıparmak
+// Copyright (C) 2024 Prof. Tufan Kumbasar, Istanbul Technical University Artificial Intelligence and Intelligent Systems (AI2S) Laboratory
+// Copyright (C) 2024 Prof. Behçet Uğur Töreyin
+//
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+#include <ilqr_trajectory_tracker/models/ackermann_robot_model.hpp>
+#include <Eigen/Dense>
+#include <cmath>
+
+using namespace Eigen;
+
+namespace ilqr_trajectory_tracker
+{
+
+AckermannRobotModel::AckermannRobotModel(const double wheelbase)
+: Model<AckermannRobotModelState, AckermannRobotModelInput>(), wheelbase_(wheelbase)
+{
+
+}
+
+AckermannRobotModelState AckermannRobotModel::applySystemDynamics(
+  const AckermannRobotModelState & x, const AckermannRobotModelInput & u,
+  const double dt)
+{
+  AckermannRobotModelState x_final;
+  x_final <<
+    x[0] + u[0] * std::cos(x[2]) * dt,
+    x[1] + u[0] * std::sin(x[2]) * dt,
+    x[2] + (u[0] / wheelbase_) * std::tan(u[1]) * dt;
+
+  return x_final;
+}
+
+AckermannRobotModelInput AckermannRobotModel::applyLimits(const AckermannRobotModelInput & u) {
+  return u.cwiseMin(input_limits_max_).cwiseMax(input_limits_min_);
+}
+
+MatrixXd AckermannRobotModel::getStateMatrix(
+  const AckermannRobotModelState & x_eq, const AckermannRobotModelInput & u_eq,
+  const double dt)
+{
+  Matrix3d state_matrix;
+  state_matrix << 1, 0, -u_eq[0] * std::sin(x_eq[2]) * dt,
+    0, 1, u_eq[0] * std::cos(x_eq[2]) * dt,
+    0, 0, 1;
+
+  return state_matrix;
+}
+
+MatrixXd AckermannRobotModel::getControlMatrix(
+  const AckermannRobotModelState & x_eq, const AckermannRobotModelInput & u_eq,
+  const double dt)
+{
+  Matrix<double, 3, 2> control_matrix;
+  control_matrix << std::cos(x_eq[2]) * dt, 0,
+    std::sin(x_eq[2]) * dt, 0,
+    std::tan(u_eq[1]) / wheelbase_ * dt, (u_eq[0] / wheelbase_) * (1 / (std::cos(u_eq[1]) * std::cos(u_eq[1]))) * dt;
+
+  return control_matrix;
+}
+
+}
diff --git a/ilqr_trajectory_tracker/src/models/diff_robot_model.cpp b/ilqr_trajectory_tracker/src/models/diff_robot_model.cpp
index 953209c..3f9125e 100644
--- a/ilqr_trajectory_tracker/src/models/diff_robot_model.cpp
+++ b/ilqr_trajectory_tracker/src/models/diff_robot_model.cpp
@@ -30,11 +30,11 @@ DiffDriveRobotModel::DiffDriveRobotModel()
 
 }
 
-Vector3d DiffDriveRobotModel::applySystemDynamics(
-  const Vector3d & x, const Vector2d & u,
+DiffDriveRobotModelState DiffDriveRobotModel::applySystemDynamics(
+  const DiffDriveRobotModelState & x, const DiffDriveRobotModelInput & u,
   const double dt)
 {
-  Vector3d x_final;
+  DiffDriveRobotModelState x_final;
   x_final <<
     x[0] + u[0] * std::cos(x[2] + u[1] * dt) * dt,
     x[1] + u[0] * std::sin(x[2] + u[1] * dt) * dt,
@@ -43,8 +43,12 @@ Vector3d DiffDriveRobotModel::applySystemDynamics(
   return x_final;
 }
 
+DiffDriveRobotModelInput DiffDriveRobotModel::applyLimits(const DiffDriveRobotModelInput & u) {
+  return u.cwiseMin(input_limits_max_).cwiseMax(input_limits_min_);
+}
+
 MatrixXd DiffDriveRobotModel::getStateMatrix(
-  const Vector3d & x_eq, const Vector2d & u_eq,
+  const DiffDriveRobotModelState & x_eq, const DiffDriveRobotModelInput & u_eq,
   const double dt)
 {
   Matrix3d state_matrix;
@@ -56,7 +60,7 @@ MatrixXd DiffDriveRobotModel::getStateMatrix(
 }
 
 MatrixXd DiffDriveRobotModel::getControlMatrix(
-  const Vector3d & x_eq, const Vector2d & /*u_eq*/,
+  const DiffDriveRobotModelState & x_eq, const DiffDriveRobotModelInput & u_eq,
   const double dt)
 {
   Matrix<double, 3, 2> control_matrix;
diff --git a/nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/frenet_ilqr_controller.hpp b/nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/frenet_ilqr_controller.hpp
index 786b130..ef4bf34 100644
--- a/nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/frenet_ilqr_controller.hpp
+++ b/nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/frenet_ilqr_controller.hpp
@@ -37,6 +37,7 @@
 #include "frenet_trajectory_planner/type_definitions.hpp"
 #include "frenet_trajectory_planner/frenet_trajectory_planner.hpp"
 #include "ilqr_trajectory_tracker/models/diff_robot_model.hpp"
+#include "ilqr_trajectory_tracker/models/ackermann_robot_model.hpp"
 #include "ilqr_trajectory_tracker/ilqr_optimizer.hpp"
 
 namespace nav2_frenet_ilqr_controller
diff --git a/nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/parameter_handler.hpp b/nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/parameter_handler.hpp
index d8cb8ad..c3624a1 100644
--- a/nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/parameter_handler.hpp
+++ b/nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/parameter_handler.hpp
@@ -37,7 +37,8 @@ struct Parameters
   double transform_tolerance;
   double time_discretization;
   int iteration_number;
-  double alpha;
+  std::string robot_type;
+  double wheelbase;
   frenet_trajectory_planner::FrenetTrajectoryPlannerConfig frenet_trajectory_planner_config;
 };
 
diff --git a/nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/policies/rclcpp_node_policy.hpp b/nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/policies/rclcpp_node_policy.hpp
index 44425c4..de52b32 100644
--- a/nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/policies/rclcpp_node_policy.hpp
+++ b/nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/policies/rclcpp_node_policy.hpp
@@ -33,7 +33,7 @@ namespace policies
 class RclcppNodePolicy : public Policy
 {
 public:
-  RclcppNodePolicy();
+  RclcppNodePolicy() : Policy() {}
   virtual void initialize(
     const std::string & policy_plugin_name,
     const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
@@ -55,11 +55,6 @@ class RclcppNodePolicy : public Policy
   nav2_costmap_2d::Costmap2D * costmap_{nullptr};
 };
 
-RclcppNodePolicy::RclcppNodePolicy()
-: Policy()
-{
-}
-
 }
 }
 
diff --git a/nav2_frenet_ilqr_controller/src/frenet_ilqr_controller.cpp b/nav2_frenet_ilqr_controller/src/frenet_ilqr_controller.cpp
index f7432b9..6d9124d 100644
--- a/nav2_frenet_ilqr_controller/src/frenet_ilqr_controller.cpp
+++ b/nav2_frenet_ilqr_controller/src/frenet_ilqr_controller.cpp
@@ -206,7 +206,7 @@ nav_msgs::msg::Path FrenetILQRController::truncateGlobalPlanWithLookAheadDist(
 }
 
 Vector2d FrenetILQRController::findOptimalInputForTrajectory(
-  const Vector3d & c_state_robot,
+  const Vector3d & x0,
   const frenet_trajectory_planner::CartesianTrajectory & robot_cartesian_trajectory)
 {
 
@@ -214,29 +214,78 @@ Vector2d FrenetILQRController::findOptimalInputForTrajectory(
   using ilqr_trajectory_tracker::DiffDriveRobotModelState;
   using ilqr_trajectory_tracker::DiffDriveRobotModelInput;
 
-  std::vector<DiffDriveRobotModelState> X_feasible;
-  for (auto cartesian_state : robot_cartesian_trajectory) {
-    DiffDriveRobotModelState x;
+  using ilqr_trajectory_tracker::AckermannRobotModel;
+  using ilqr_trajectory_tracker::AckermannRobotModelState;
+  using ilqr_trajectory_tracker::AckermannRobotModelInput;
+
+  std::vector<AckermannRobotModelState> X_feasible;
+  for (size_t i = 1; i < robot_cartesian_trajectory.size(); ++i) {
+    auto cartesian_state = robot_cartesian_trajectory[i];
+    AckermannRobotModelState x;
     x << cartesian_state[0],
       cartesian_state[3],
       cartesian_state[6];
     X_feasible.push_back(x);
   }
 
-  Matrix3d Q = Matrix3d::Identity() * 10;
-  Matrix2d R = Matrix2d::Identity() * 2;
-  double alpha = 1;
-  double dt = 0.05;
-  ilqr_trajectory_tracker::NewtonOptimizer<DiffDriveRobotModel> newton_optimizer;
-  newton_optimizer.setIterationNumber(20);
-  newton_optimizer.setAlpha(alpha);
-  auto U_optimal = newton_optimizer.optimize(c_state_robot, X_feasible, Q, R, dt);
-
-  if (U_optimal.empty()) {
-    throw nav2_core::NoValidControl("Iterative LQR couldn't find any solution!");
-  }
+  Matrix3d Q = Matrix3d::Identity() * 1;
+  Matrix2d R = Matrix2d::Identity() * 0.2;
+
+  if (params_->robot_type == "ackermann") {
+    using ilqr_trajectory_tracker::AckermannRobotModel;
+    ilqr_trajectory_tracker::NewtonOptimizer<AckermannRobotModel> newton_optimizer(params_->wheelbase);
+
+    Vector2d input_limits_min;
+    input_limits_min << -0.5, -0.523599;
+
+    Vector2d input_limits_max;
+    input_limits_max << 0.5, 0.523599;
+    newton_optimizer.setInputConstraints(input_limits_min, input_limits_max);
+
+    newton_optimizer.setIterationNumber(20);
+    auto U_optimal = newton_optimizer.optimize(x0, X_feasible, Q, R, params_->time_discretization);
+
+    RCLCPP_INFO(logger_, "--------------------");
+    for (auto cstate : X_feasible) {
+      RCLCPP_INFO(logger_, "[%f, %f, %f]", cstate[0], cstate[1], cstate[2]);
+    }
+    RCLCPP_INFO(logger_, "********************");
+
+    if (U_optimal.empty()) {
+      throw nav2_core::NoValidControl("Iterative LQR couldn't find any solution!");
+    }
 
-  return U_optimal[0];
+    Vector2d u_ackermann;
+    u_ackermann << U_optimal[0][0], (U_optimal[0][0] / params_->wheelbase) * std::tan(U_optimal[0][1]);
+    RCLCPP_INFO(logger_, "--------------------");
+    for (auto u : U_optimal) {
+      RCLCPP_INFO(logger_, "[%f, %f]", u[0], u[1]);
+    }
+    RCLCPP_INFO(logger_, "********************");
+
+    return u_ackermann;
+  } else if (params_->robot_type == "diffdrive") {
+    using ilqr_trajectory_tracker::DiffDriveRobotModel;
+    ilqr_trajectory_tracker::NewtonOptimizer<DiffDriveRobotModel> newton_optimizer;
+
+    Vector2d input_limits_min;
+    input_limits_min << -0.5, -2.5;
+
+    Vector2d input_limits_max;
+    input_limits_max << 0.5, 2.5;
+    newton_optimizer.setInputConstraints(input_limits_min, input_limits_max);
+
+    newton_optimizer.setIterationNumber(20);
+    auto U_optimal = newton_optimizer.optimize(x0, X_feasible, Q, R, params_->time_discretization);
+
+    if (U_optimal.empty()) {
+      throw nav2_core::NoValidControl("Iterative LQR couldn't find any solution!");
+    }
+
+    return U_optimal[0];
+  } else {
+    throw nav2_core::NoValidControl("Unknown robot type! Possible robot types : ackermann, diffdrive");
+  }
 }
 
 geometry_msgs::msg::TwistStamped FrenetILQRController::computeVelocityCommands(
@@ -277,14 +326,20 @@ geometry_msgs::msg::TwistStamped FrenetILQRController::computeVelocityCommands(
   frenet_trajectory_planner::CartesianState robot_cartesian_state =
     frenet_trajectory_planner::CartesianState::Zero();
   double robot_yaw = tf2::getYaw(robot_pose.pose.orientation);
-  double linear_speed = speed.linear.x;
-  if (linear_speed == 0) {
-    linear_speed = 0.001;
-  }
+  double linear_speed = std::abs(speed.linear.x);
+  
+  // if (std::abs(linear_speed) <= 0.5) {
+  //   params_->frenet_trajectory_planner_config.time_interval = 10;
+  // }
+  // double max_curvature = 2.5;
+  // linear_speed = 1 / max_curvature;
   robot_cartesian_state[0] = robot_pose.pose.position.x;
   robot_cartesian_state[1] = linear_speed * std::cos(robot_yaw);
+  // robot_cartesian_state[2] = -linear_speed * std::sin(robot_yaw);
   robot_cartesian_state[3] = robot_pose.pose.position.y;
   robot_cartesian_state[4] = linear_speed * std::sin(robot_yaw);
+  // robot_cartesian_state[5] = linear_speed * std::cos(robot_yaw);
+  robot_cartesian_state[6] = robot_yaw;
 
   frenet_trajectory_planner_.setFrenetTrajectoryPlannerConfig(
     params_->frenet_trajectory_planner_config);
diff --git a/nav2_frenet_ilqr_controller/src/parameter_handler.cpp b/nav2_frenet_ilqr_controller/src/parameter_handler.cpp
index 18c2b46..89a3466 100644
--- a/nav2_frenet_ilqr_controller/src/parameter_handler.cpp
+++ b/nav2_frenet_ilqr_controller/src/parameter_handler.cpp
@@ -78,8 +78,9 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".ilqr_trajectory_tracker.iteration_number", rclcpp::ParameterValue(20));
 
   declare_parameter_if_not_declared(
-    node, plugin_name_ + ".ilqr_trajectory_tracker.alpha", rclcpp::ParameterValue(1.0));
-
+    node, plugin_name_ + ".robot_type", rclcpp::ParameterValue("diff_drive"));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".wheelbase", rclcpp::ParameterValue(2.5));
 
   node->get_parameter(
     plugin_name_ + ".interpolate_curvature_after_goal",
@@ -122,7 +123,9 @@ ParameterHandler::ParameterHandler(
   node->get_parameter(
     plugin_name_ + ".ilqr_trajectory_tracker.iteration_number",
     params_.iteration_number);
-  node->get_parameter(plugin_name_ + ".ilqr_trajectory_tracker.alpha", params_.alpha);
+
+  node->get_parameter(plugin_name_ + ".robot_type", params_.robot_type);
+  node->get_parameter(plugin_name_ + ".wheelbase", params_.wheelbase);
 
 
   dynamic_params_handler_ = node->add_on_set_parameters_callback(
@@ -184,8 +187,10 @@ ParameterHandler::dynamicParametersCallback(
       } else {
         params_.iteration_number = parameter.as_int();
       }
-    } else if (name == plugin_name_ + ".ilqr_trajectory_tracker.alpha") {
-      params_.alpha = parameter.as_double();
+    } else if (name == plugin_name_ + ".robot_type") {
+      params_.robot_type = parameter.as_string();
+    } else if (name == plugin_name_ + ".wheelbase") {
+      params_.wheelbase = parameter.as_double();
     }
   }
 
diff --git a/nav2_frenet_ilqr_controller/src/policies/rclcpp_node_policy.cpp b/nav2_frenet_ilqr_controller/src/policies/rclcpp_node_policy.cpp
new file mode 100644
index 0000000..8d1e12b
--- /dev/null
+++ b/nav2_frenet_ilqr_controller/src/policies/rclcpp_node_policy.cpp
@@ -0,0 +1,28 @@
+#include "frenet_trajectory_planner/policies/rclcpp_node_policy.hpp"
+
+using namespace frenet_trajectory_planner::policies;
+using namespace frenet_trajectory_planner;
+
+namespace nav2_frenet_ilqr_controller
+{
+namespace policies
+{
+
+RclcppNodePolicy::RclcppNodePolicy()
+: Policy()
+{
+}
+
+void RclcppNodePolicy::initialize(
+  const std::string & policy_plugin_name,
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  policy_plugin_name_ = policy_plugin_name;
+  node_ = parent;
+  costmap_ros_ = costmap_ros;
+  costmap_ = costmap_ros_->getCostmap();
+}
+
+}
+}
\ No newline at end of file