diff --git a/config/planning/pacsim.yaml b/config/planning/pacsim.yaml
index a1dd9700b..4fdafcbc2 100644
--- a/config/planning/pacsim.yaml
+++ b/config/planning/pacsim.yaml
@@ -16,10 +16,10 @@ planning:
   smoothing_spline_coeffs_ratio: 3.0
   smoothing_spline_precision: 10 
   publishing_visualization_msg: true
-  pre_defined_velocity_planning: 4.0
+  pre_defined_velocity_planning: 2.0
   use_outlier_removal: false
   use_path_smoothing: true
   minimum_velocity: 2.0
   braking_acceleration: -1.0
   normal_acceleration: 4.0
-  use_velocity_planning: true
\ No newline at end of file
+  use_velocity_planning: false
\ No newline at end of file
diff --git a/src/control/src/adapter_control/pacsim.cpp b/src/control/src/adapter_control/pacsim.cpp
index 823d9fa82..079ca7bc3 100644
--- a/src/control/src/adapter_control/pacsim.cpp
+++ b/src/control/src/adapter_control/pacsim.cpp
@@ -25,8 +25,8 @@ PacSimAdapter::PacSimAdapter(const ControlParameters& params)
     car_velocity_sub_ = this->create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
         "/pacsim/velocity", 10, [this](const geometry_msgs::msg::TwistWithCovarianceStamped& msg) {
           velocity_ = std::sqrt(std::pow(msg.twist.twist.linear.x, 2) +
-                                std::pow(msg.twist.twist.linear.y, 2) +
-                                std::pow(msg.twist.twist.linear.z, 2));
+                                            std::pow(msg.twist.twist.linear.y, 2) +
+                                            std::pow(msg.twist.twist.linear.z, 2));
         });
   }
 
@@ -50,7 +50,8 @@ void PacSimAdapter::timer_callback() {
     pose.x = t.transform.translation.x;
     pose.y = t.transform.translation.y;
 
-    RCLCPP_DEBUG(get_logger(), "Pose info. Position:%f, %f, Theta %f", pose.x, pose.y, pose.theta);
+    RCLCPP_DEBUG(get_logger(), "Pose info. Position:%f, %f, Theta %f", pose.x,
+                 pose.y, pose.theta);
     this->publish_control(pose);
   }
 }
diff --git a/src/control/src/node_/node_control.cpp b/src/control/src/node_/node_control.cpp
index d5c1e97f9..d7524977c 100644
--- a/src/control/src/node_/node_control.cpp
+++ b/src/control/src/node_/node_control.cpp
@@ -102,6 +102,8 @@ Control::Control(const ControlParameters& params)
 
 // This function is called when a new pose is received
 void Control::publish_control(const custom_interfaces::msg::Pose& vehicle_state_msg) {
+
+  RCLCPP_INFO(rclcpp::get_logger("control"), "Velocity: %f", this->velocity_);
   if (!go_signal_) {
     RCLCPP_INFO(rclcpp::get_logger("control"), "Go Signal Not received");
 
@@ -147,8 +149,7 @@ void Control::publish_control(const custom_interfaces::msg::Pose& vehicle_state_
   }
 
   // calculate longitudinal control: PI-D
-  double torque = this->long_controller_.update(closest_point_velocity,
-                                                this->point_solver_.vehicle_pose_.velocity_);
+  double torque = this->long_controller_.update(lookahead_velocity, this->velocity_);
 
   // calculate Lateral Control: Pure Pursuit
   double steering_angle = this->lat_controller_.pp_steering_control_law(
diff --git a/src/launcher/launch/slam_pacsim.launch.py b/src/launcher/launch/slam_pacsim.launch.py
index 48f26bc1a..de0cc5bf8 100644
--- a/src/launcher/launch/slam_pacsim.launch.py
+++ b/src/launcher/launch/slam_pacsim.launch.py
@@ -25,10 +25,20 @@ def generate_launch_description():
             )
         ),
     )
+
+    control_launch_description = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("control"), "launch", "control.launch.py"]
+            )
+        ),
+    )
+
     return LaunchDescription(
         [
             slam_launch_description,
             pacsim_launch_description,
             evaluator_launch_description,
+            control_launch_description
         ],
     )
diff --git a/src/launcher/launch/state_est_pacsim.launch.py b/src/launcher/launch/state_est_pacsim.launch.py
index 333b21e76..a190d0bc3 100644
--- a/src/launcher/launch/state_est_pacsim.launch.py
+++ b/src/launcher/launch/state_est_pacsim.launch.py
@@ -36,11 +36,29 @@ def generate_launch_description():
             )
         ),
     )
+
+    planning_launch_description = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("planning"), "launch", "planning.launch.py"]
+            )
+        ),
+    )
+    control_launch_description = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("control"), "launch", "control.launch.py"]
+            )
+    ),)
+
+    
     return LaunchDescription(
         [
             slam_launch_description,
             ve_launch_description,
             pacsim_launch_description,
             evaluator_launch_description,
+            planning_launch_description,
+            control_launch_description,
         ],
     )
diff --git a/src/pacsim/config/vehicleModel.yaml b/src/pacsim/config/vehicleModel.yaml
index f6482843a..17e40983d 100644
--- a/src/pacsim/config/vehicleModel.yaml
+++ b/src/pacsim/config/vehicleModel.yaml
@@ -18,8 +18,8 @@ vehicle_model:
     Izz: 101.082
     wheelRadius: 0.203
     gearRatio: 4.0
-    innerSteeringRatio: 0.1869
-    outerSteeringRatio: 0.1582
+    innerSteeringRatio: 0.255625
+    outerSteeringRatio: 0.20375
     nominalVoltageTS: 600.0
     powerGroundForce: 0.0
     # powertrainEfficiency: 0.95
\ No newline at end of file
diff --git a/src/pacsim/include/VehicleModel/VehicleModelBicycle.hpp b/src/pacsim/include/VehicleModel/VehicleModelBicycle.hpp
index 3aa1eb310..f856c281e 100644
--- a/src/pacsim/include/VehicleModel/VehicleModelBicycle.hpp
+++ b/src/pacsim/include/VehicleModel/VehicleModelBicycle.hpp
@@ -56,7 +56,7 @@ class VehicleModelBicycle : public IVehicleModel {
     void setOrientation(Eigen::Vector3d orientation) override;
     
     // Simulation methods
-    void forwardIntegrate(double dt) override;
+    void forwardIntegrate(double dt, Wheels frictionCoefficients);
     std::array<Eigen::Vector3d, 4> getWheelPositions() override;
 
 private:
@@ -217,7 +217,7 @@ class VehicleModelBicycle : public IVehicleModel {
         const Eigen::Vector3d& vRL, const Eigen::Vector3d& vRR);
 
     // Physics calculation methods
-    Eigen::Vector3d getDynamicStates(double dt);
+    Eigen::Vector3d getDynamicStates(double dt, Wheels frictionCoefficients);
     void calculateNormalForces(double& Fz_Front, double& Fz_Rear) const;
     void calculateSlipAngles(double& kappaFront, double& kappaRear) const;
     void updateWheelSpeeds(double dt);
diff --git a/src/pacsim/include/VehicleModel/VehicleModelInterface.hpp b/src/pacsim/include/VehicleModel/VehicleModelInterface.hpp
index f4223c98b..a4757fad7 100644
--- a/src/pacsim/include/VehicleModel/VehicleModelInterface.hpp
+++ b/src/pacsim/include/VehicleModel/VehicleModelInterface.hpp
@@ -43,6 +43,6 @@ class IVehicleModel {
     virtual void setOrientation(Eigen::Vector3d orientation) = 0;
     
     // Simulation methods
-    virtual void forwardIntegrate(double dt) = 0;
+    virtual void forwardIntegrate(double dt, Wheels frictionCoefficients) = 0;
     virtual std::array<Eigen::Vector3d, 4> getWheelPositions() = 0;
 };
\ No newline at end of file
diff --git a/src/pacsim/src/VehicleModel/VehicleModelBicycle.cpp b/src/pacsim/src/VehicleModel/VehicleModelBicycle.cpp
index 27f28d10b..0da000bd8 100644
--- a/src/pacsim/src/VehicleModel/VehicleModelBicycle.cpp
+++ b/src/pacsim/src/VehicleModel/VehicleModelBicycle.cpp
@@ -87,14 +87,25 @@ void VehicleModelBicycle::setMaxTorques(Wheels in) { maxTorques = in; }
 void VehicleModelBicycle::setMinTorques(Wheels in) { minTorques = in; }
 
 void VehicleModelBicycle::setSteeringSetpointFront(double in) {
-  steeringModel.calculateSteeringAngles(in, steeringAngles);
+        double avgRatio = 0.5 * (this->steeringModel.innerSteeringRatio + this->steeringModel.outerSteeringRatio);
+        if (in > 0)
+        {
+            this->steeringAngles.FL = this->steeringModel.innerSteeringRatio * in;
+            this->steeringAngles.FR = this->steeringModel.outerSteeringRatio * in;
+        }
+        else
+        {
+            this->steeringAngles.FL = this->steeringModel.outerSteeringRatio * in;
+            this->steeringAngles.FR = this->steeringModel.innerSteeringRatio * in;
+        }
+        return;
 }
 
 void VehicleModelBicycle::setSteeringSetpointRear(double in) {
   // Rear steering not implemented
 }
 
-void VehicleModelBicycle::setThrottle(double in) { throttleActuation = std::clamp(in, -1.0, 1.0); }
+void VehicleModelBicycle::setThrottle(double in) { throttleActuation = 0.1 * std::clamp(in, -1.0, 1.0); }
 
 void VehicleModelBicycle::setPowerGroundSetpoint(double in) {
   powerGroundSetpoint = std::clamp(in, 0.0, 1.0);
@@ -106,8 +117,8 @@ void VehicleModelBicycle::setOrientation(Eigen::Vector3d newOrientation) {
   orientation = newOrientation;
 }
 
-double VehicleModelBicycle::processSlipAngleLat(double alpha_input, double Fz) {
-  return tireModel.calculateLateralForce(alpha_input, Fz);
+double VehicleModelBicycle::processSlipAngleLat(double alpha_input, double Fz = 0.0) {
+  return std::sin(tireModel.Clat * std::atan(tireModel.Blat * alpha_input - tireModel.Elat * (tireModel.Blat * alpha_input - std::atan(tireModel.Blat * alpha_input))));
 }
 
 void VehicleModelBicycle::AerodynamicsModel::calculateForces(double velocityX, double& downforce,
@@ -124,48 +135,6 @@ int VehicleModelBicycle::TireModel::sign(double value) const {
   return 1;  // Default to 1 for zero
 }
 
-double VehicleModelBicycle::TireModel::calculateLateralForce(double slipAngle,
-                                                             double normalForce) const {
-  double dpi = (p_input - NOMPRES) / NOMPRES;
-  double Fz_0_prime = LFZO * FNOM;
-  double Kya = PKY1 * Fz_0_prime * (1 + PPY1 * dpi) * (1 - PKY3 * abs(y_input)) *
-               sin(PKY4 * atan((normalForce / Fz_0_prime) /
-                               ((PKY2 + PKY5 * y_input * y_input) * (1 + PPY2 * dpi)))) *
-               zeta_3 * LKY;
-  double dfz = (normalForce - Fz_0_prime) / Fz_0_prime;
-  double Kyg0 = normalForce * (PKY6 + PKY7 * dfz) * (1 + PPY5 * dpi) * LKYC;
-  double Vs_y = tan(slipAngle) * abs(V_cx);
-  double Vs_x = -slip_ratio * abs(V_cx);
-  double Vs = sqrt(Vs_x * Vs_x + Vs_y * Vs_y);
-  double V0 = LONGVL;
-  double LMUY_star = LMUY / (1.0 + LMUV * (Vs / V0));
-  double SVyg = normalForce * (PVY3 + PVY4 * dfz) * y_input * LKYC * LMUY_star * zeta_2;
-  double SHy = (PHY1 + PHY2 * dfz) +
-               ((Kyg0 * y_input - SVyg) / (Kya + eps_k * sign(Kya))) * zeta_0 + zeta_4 - 1.0;
-  double alpha_y = slipAngle + SHy;
-  double Ey = (PEY1 + PEY2 * dfz) *
-              (1 + PEY5 * y_input * y_input - (PEY3 + PEY4 * y_input) * sign(alpha_y)) * LEY;
-  double Cy = PCY1 * LCY;
-  double mu_y = (PDY1 + PDY2 * dfz) * (1.0 + PPY3 * dpi + PPY4 * dpi * dpi) *
-                (1.0 - PDY3 * y_input * y_input) * LMUY_star;
-  double Dy = mu_y * normalForce * zeta_2;
-  double By = Kya / (Cy * Dy + eps_y * sign(Dy));
-  double DVyk = mu_y * normalForce * (RVY1 + RVY2 * dfz + RVY3 * y_input) *
-                cos(atan(RVY4 * slipAngle)) * zeta_2;
-  double SVyk = DVyk * sin(RVY5 * atan(RVY6 * slip_ratio)) * LVYKA;
-  double Eyk = REY1 + REY2 * dfz;
-  double SHyk = RHY1 + RHY2 * dfz;
-  double ks = slip_ratio + SHyk;
-  double Byk = (RBY1 + RBY4 * y_input * y_input) * cos(atan(RBY2 * (slipAngle - RBY3))) * LYKA;
-  double Cyk = RCY1;
-  double Gyk_0 = cos(Cyk * atan(Byk * SHyk - Eyk * (Byk * SHyk - atan(Byk * SHyk))));
-  double Gyk = (cos(Cyk * atan(Byk * ks - Eyk * (Byk * ks - atan(Byk * ks)))) / Gyk_0);
-  double SVy = normalForce * (PVY1 + PVY2 * dfz) * LVY * LMUY_star * zeta_2 + SVyg;
-  double Fy_0 = Dy * sin(Cy * atan(By * alpha_y - Ey * (By * alpha_y - atan(By * alpha_y)))) + SVy;
-  double Fy = Gyk * Fy_0 + SVyk;
-
-  return Fy;
-}
 
 void VehicleModelBicycle::PowertrainModel::calculateWheelTorques(double throttleInput,
                                                                  Wheels& torques) const {
@@ -272,11 +241,11 @@ void VehicleModelBicycle::calculateSlipAngles(double& kappaFront, double& kappaR
 
 // New helper function to calculate wheel positions and velocities
 void VehicleModelBicycle::calculateWheelGeometry(double& steeringFront, Eigen::Vector3d& rFL,
-                                                 Eigen::Vector3d& rFR, Eigen::Vector3d& rRL,
-                                                 Eigen::Vector3d& rRR, Eigen::Vector3d& rFront,
-                                                 Eigen::Vector3d& rRear, Eigen::Vector3d& vFL,
-                                                 Eigen::Vector3d& vFR, Eigen::Vector3d& vRL,
-                                                 Eigen::Vector3d& vRR) const {
+                         Eigen::Vector3d& rFR, Eigen::Vector3d& rRL,
+                         Eigen::Vector3d& rRR, Eigen::Vector3d& rFront,
+                         Eigen::Vector3d& rRear, Eigen::Vector3d& vFL,
+                         Eigen::Vector3d& vFR, Eigen::Vector3d& vRL,
+                         Eigen::Vector3d& vRR) const {
   // Calculate average front steering angle
   steeringFront = 0.5 * (steeringAngles.FL + steeringAngles.FR);
 
@@ -297,7 +266,7 @@ void VehicleModelBicycle::calculateWheelGeometry(double& steeringFront, Eigen::V
 
 // New helper function to calculate longitudinal forces
 void VehicleModelBicycle::calculateLongitudinalForces(double& Fx_FL, double& Fx_FR, double& Fx_RL,
-                                                      double& Fx_RR) const {
+                            double& Fx_RR) const {
   // Calculate powertrain efficiency
   double powertrainEfficiency = powertrainModel.calculateEfficiency(torques);
 
@@ -305,9 +274,9 @@ void VehicleModelBicycle::calculateLongitudinalForces(double& Fx_FL, double& Fx_
   Fx_FL = 0;  // Front wheel drive not enabled
   Fx_FR = 0;  // Front wheel drive not enabled
   Fx_RL =
-      (powertrainModel.gearRatio * torques.RL / powertrainModel.wheelRadius) * powertrainEfficiency;
+    (powertrainModel.gearRatio * torques.RL / powertrainModel.wheelRadius) * powertrainEfficiency;
   Fx_RR =
-      (powertrainModel.gearRatio * torques.RR / powertrainModel.wheelRadius) * powertrainEfficiency;
+    (powertrainModel.gearRatio * torques.RR / powertrainModel.wheelRadius) * powertrainEfficiency;
 
   // Apply forces only when torque is significant or vehicle is moving
   Fx_FL *= (torques.FL > TORQUE_THRESHOLD || velocity.x() > VELOCITY_MIN_THRESHOLD) ? 1.0 : 0.0;
@@ -318,30 +287,30 @@ void VehicleModelBicycle::calculateLongitudinalForces(double& Fx_FL, double& Fx_
 
 // New helper function to calculate accelerations
 Eigen::Vector3d VehicleModelBicycle::calculateAccelerations(double steeringFront, double Fx_FL,
-                                                            double Fx_FR, double Fx_RL,
-                                                            double Fx_RR, double Fy_Front,
-                                                            double Fy_Rear, double drag,
-                                                            const Eigen::Vector3d& friction) const {
+                              double Fx_FR, double Fx_RL,
+                              double Fx_RR, double Fy_Front,
+                              double Fy_Rear, double drag,
+                              const Eigen::Vector3d& friction) const {
   // Calculate longitudinal acceleration from tire forces
   double axTires = (std::cos(steeringAngles.FL) * Fx_FL + std::cos(steeringAngles.FR) * Fx_FR +
-                    Fx_RL + Fx_RR - std::sin(steeringFront) * Fy_Front) /
-                   m;
+          Fx_RL + Fx_RR - std::sin(steeringFront) * Fy_Front) /
+           m;
 
   // Apply aerodynamic drag and friction to get total acceleration
   double axModel = axTires + drag / m + friction.x() / m;
 
   // Calculate lateral acceleration from tire forces
   double ayTires = (std::sin(steeringAngles.FL) * Fx_FL + std::sin(steeringAngles.FR) * Fx_FR +
-                    std::cos(steeringFront) * Fy_Front + Fy_Rear) /
-                   m;
+          std::cos(steeringFront) * Fy_Front + Fy_Rear) /
+           m;
   double ayModel = ayTires + friction.y() / m;
 
   // Calculate yaw moment contributions
   double rdotFx
-      = 0.5 * this->sf * (-Fx_FL * std::cos(this->steeringAngles.FL) + Fx_FR * std::cos(this->steeringAngles.FR))
-      + this->lf * (Fx_FL * std::sin(this->steeringAngles.FL) + Fx_FR * std::sin(this->steeringAngles.FR))
-      + 0.5 * this->sr * (Fx_RR * std::cos(this->steeringAngles.RR) - Fx_RL * std::cos(this->steeringAngles.RL))
-      - this->lr * (Fx_RL * std::sin(this->steeringAngles.RL) + Fx_RR * std::sin(this->steeringAngles.RR));
+    = 0.5 * this->sf * (-Fx_FL * std::cos(this->steeringAngles.FL) + Fx_FR * std::cos(this->steeringAngles.FR))
+    + this->lf * (Fx_FL * std::sin(this->steeringAngles.FL) + Fx_FR * std::sin(this->steeringAngles.FR))
+    + 0.5 * this->sr * (Fx_RR * std::cos(this->steeringAngles.RR) - Fx_RL * std::cos(this->steeringAngles.RL))
+    - this->lr * (Fx_RL * std::sin(this->steeringAngles.RL) + Fx_RR * std::sin(this->steeringAngles.RR));
 
   double rdotFy = lf * (Fy_Front * std::cos(steeringFront)) - lr * (Fy_Rear);
 
@@ -365,58 +334,122 @@ void VehicleModelBicycle::updateWheelSpeeds(const Eigen::Vector3d& vFL, const Ei
   wheelspeeds.RR = vRR.x() / rpm2ms;
 }
 
-// Refactored getDynamicStates using the new helper functions
-Eigen::Vector3d VehicleModelBicycle::getDynamicStates(double dt) {
-  // Calculate wheel positions and velocities
-  double steeringFront;
-  Eigen::Vector3d rFL, rFR, rRL, rRR, rFront, rRear;
-  Eigen::Vector3d vFL, vFR, vRL, vRR;
-  calculateWheelGeometry(steeringFront, rFL, rFR, rRL, rRR, rFront, rRear, vFL, vFR, vRL, vRR);
-
-  // Calculate normal forces on axles
-  double Fz_Front, Fz_Rear;
-  calculateNormalForces(Fz_Front, Fz_Rear);
-
-  // Calculate aero drag
-  double downforce, drag;
-  aeroModel.calculateForces(velocity.x(), downforce, drag);
-
-  // Calculate rolling resistance (applied opposite to velocity direction)
-  double frict = (Fz_Front + Fz_Rear) * ROLLING_RESISTANCE_COEFF;
-  Eigen::Vector3d friction;
-
-  if (velocity.norm() < 0.01) {
-      friction = Eigen::Vector3d::Zero();
-  } else {
-      Eigen::Vector3d velDir = -velocity.normalized();
-      friction = frict * velDir;
-  }
-
-  // Calculate longitudinal forces
-  double Fx_FL, Fx_FR, Fx_RL, Fx_RR;
-  calculateLongitudinalForces(Fx_FL, Fx_FR, Fx_RL, Fx_RR);
-
-  // Calculate slip angles
-  double kappaFront, kappaRear;
-  calculateSlipAngles(kappaFront, kappaRear);
-
-  // Calculate lateral forces using tire model
-  double Fy_Front = processSlipAngleLat(kappaFront, Fz_Front / 2) * 2;
-  double Fy_Rear = processSlipAngleLat(kappaRear, Fz_Rear / 2) * 2;
-
-  // Update wheel speeds
-  updateWheelSpeeds(vFL, vFR, vRL, vRR);
-
-  RCLCPP_DEBUG(rclcpp::get_logger("a"),
-      "Vx: %f, Vy: %f, W: %f, KappaF: %f, KappaR: %f, Fy_Front: %f, Fy_Rear: %f, drag: %f, friction: %f", velocity.x(),
-      velocity.y(), angularVelocity.z(), kappaFront, kappaRear, Fy_Front, Fy_Rear, drag, friction.x());
-
-  // Calculate accelerations
-  return calculateAccelerations(steeringFront, Fx_FL, Fx_FR, Fx_RL, Fx_RR, Fy_Front, Fy_Rear, drag,
-                                friction);
-}
 
-void VehicleModelBicycle::forwardIntegrate(double dt) {
+    Eigen::Vector3d VehicleModelBicycle::getDynamicStates(double dt, Wheels frictionCoefficients)
+    {
+        double l = this->lr + this->lf;
+        double vx = this->velocity.x();
+        double vy = this->velocity.y();
+        double ax = this->acceleration.x();
+        double ay = this->acceleration.y();
+        double r = this->angularVelocity.z();
+        // Downforce
+        double F_aero_downforce
+            = 0. * 1.29 * this->aeroModel.aeroArea * this->aeroModel.cla * (vx * vx) + this->powerGroundSetpoint * this->powertrainModel.powerGroundForce;
+        double F_aero_drag = 0.5 * 1.29 * this->aeroModel.aeroArea * this->aeroModel.cda * (vx * vx);
+        double g = 9.81;
+        double steeringFront = 0.5 * (this->steeringAngles.FL + this->steeringAngles.FR);
+
+        // max because lifted tire makes no forces
+        double Fz_Front = std::max(0.0, ((m * g + F_aero_downforce) * 0.5 * this->lr / l));
+        double Fz_Rear = std::max(0.0, ((m * g + F_aero_downforce) * 0.5 * this->lf / l));
+
+        Eigen::Vector3d vCog = this->velocity;
+        Eigen::Vector3d omega = this->angularVelocity;
+
+        Eigen::Vector3d rFL = Eigen::Vector3d(lf, 0.5 * sf, 0.0);
+        Eigen::Vector3d rFR = Eigen::Vector3d(lf, -0.5 * sf, 0.0);
+        Eigen::Vector3d rRL = Eigen::Vector3d(-lr, 0.5 * sr, 0.0);
+        Eigen::Vector3d rRR = Eigen::Vector3d(-lr, -0.5 * sr, 0.0);
+        Eigen::Vector3d rFront = Eigen::Vector3d(lf, 0.0, 0.0);
+        Eigen::Vector3d rRear = Eigen::Vector3d(-lf, 0.0, 0.0);
+
+        Eigen::Vector3d vFL = vCog + omega.cross(rFL);
+        Eigen::Vector3d vFR = vCog + omega.cross(rFR);
+        Eigen::Vector3d vRL = vCog + omega.cross(rRL);
+        Eigen::Vector3d vRR = vCog + omega.cross(rRR);
+        Eigen::Vector3d vFront = vCog + omega.cross(rFront);
+        Eigen::Vector3d vRear = vCog + omega.cross(rRear);
+
+        double rpm2ms = this->powertrainModel.wheelRadius * TWO_PI / 60.0;
+
+        bool stillstand = (vCog.norm() < 0.1) && (std::abs(this->angularVelocity.z()) < 0.001);
+
+        // tire side slip angles
+        double eps = 0.00001;
+
+        double kappaFront = std::atan2(vFront.y(), std::max(std::abs(vFront.x()), eps)) - steeringFront;
+        double kappaRear = std::atan2(vRear.y(), std::max(std::abs(vRear.x()), eps));
+
+        // don't steer when vehicle doesn't move
+        if (stillstand)
+        {
+            kappaFront = 0.0;
+            kappaRear = 0.0;
+        }
+
+        // old calculation as backup for now
+        double Fx_FL = this->powertrainModel.gearRatio * this->torques.FL / this->powertrainModel.wheelRadius;
+        double Fx_FR = this->powertrainModel.gearRatio * this->torques.FR / this->powertrainModel.wheelRadius;
+        double Fx_RL = this->powertrainModel.gearRatio * this->torques.RL / this->powertrainModel.wheelRadius;
+        double Fx_RR = this->powertrainModel.gearRatio * this->torques.RR / this->powertrainModel.wheelRadius;
+
+        Fx_FL *= (((this->torques.FL) > 0.5) || (vCog.x() > 0.3)) ? 1.0 : 0.0;
+        Fx_FR *= (((this->torques.FR) > 0.5) || (vCog.x() > 0.3)) ? 1.0 : 0.0;
+        Fx_RL *= (((this->torques.RL) > 0.5) || (vCog.x() > 0.3)) ? 1.0 : 0.0;
+        Fx_RR *= (((this->torques.RR) > 0.5) || (vCog.x() > 0.3)) ? 1.0 : 0.0;
+
+        double Dlat_Front = 0.5 * (frictionCoefficients.FL + frictionCoefficients.FR) * this->tireModel.Dlat * Fz_Front;
+        double Dlat_Rear = 0.5 * (frictionCoefficients.RL + frictionCoefficients.RR) * this->tireModel.Dlat * Fz_Rear;
+
+        double Fy_Front = Dlat_Front * processSlipAngleLat(kappaFront);
+        double Fy_Rear = Dlat_Rear * processSlipAngleLat(kappaRear);
+
+        // todo convert speed
+        this->wheelspeeds.FL = vFL.x() / rpm2ms;
+        this->wheelspeeds.FR = vFR.x() / rpm2ms;
+        this->wheelspeeds.RL = vRL.x() / rpm2ms;
+        this->wheelspeeds.RR = vRR.x() / rpm2ms;
+
+        double axTires = (std::cos(this->steeringAngles.FL) * Fx_FL + std::cos(this->steeringAngles.FR) * Fx_FR + Fx_RL
+                             + Fx_RR - std::sin(steeringFront) * Fy_Front)
+            / m;
+        double axModel = axTires - F_aero_drag / m;
+
+        double ayTires = (std::sin(this->steeringAngles.FL) * Fx_FL + std::sin(this->steeringAngles.FR) * Fx_FR
+                             + std::cos(steeringFront) * Fy_Front + Fy_Rear)
+            / m;
+        double ayModel = (ayTires);
+
+        double rdotFx
+            = 0.5 * this->sf * (-Fx_FL * std::cos(this->steeringAngles.FL) + Fx_FR * std::cos(this->steeringAngles.FR))
+            + this->lf * (Fx_FL * std::sin(this->steeringAngles.FL) + Fx_FR * std::sin(this->steeringAngles.FR))
+            + 0.5 * this->sr * (Fx_RR * std::cos(this->steeringAngles.RR) - Fx_RL * std::cos(this->steeringAngles.RL))
+            - this->lr * (Fx_RL * std::sin(this->steeringAngles.RL) + Fx_RR * std::sin(this->steeringAngles.RR));
+        double rdotFy = this->lf * (Fy_Front * std::cos(steeringFront)) - this->lr * (Fy_Rear);
+        double rdot = (1 / Izz * (rdotFx + rdotFy));
+
+        Eigen::Vector3d ret(axModel, ayModel, rdot);
+        return ret;
+    }
+
+void VehicleModelBicycle::forwardIntegrate(double dt, Wheels frictionCoefficients) {
+
+  // Override dt with the difference of timestamps using the system clock
+  //double dt1 = dt;
+  
+  static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now();
+  auto now = std::chrono::steady_clock::now();
+  std::chrono::duration<double> elapsed = now - last_time;
+  dt = elapsed.count();
+  last_time = now;
+
+  Eigen::Vector3d friction(std::min(200.0, 2000.0 * std::abs(this->velocity.x())),
+    std::min(200.0, 2000.0 * std::abs(this->velocity.y())),
+    std::min(200.0, 2000.0 * std::abs(this->velocity.z())));
+  friction[0] = (this->velocity.x() > 0) ? friction.x() : -friction.x();
+  friction[1] = (this->velocity.y() > 0) ? friction.y() : -friction.y();
+  friction[2] = (this->velocity.z() > 0) ? friction.z() : -friction.z();
   // Update position based on velocity and orientation
   Eigen::AngleAxisd yawAngle(orientation.z(), Eigen::Vector3d::UnitZ());
   position += (yawAngle.matrix() * velocity) * dt;
@@ -425,10 +458,10 @@ void VehicleModelBicycle::forwardIntegrate(double dt) {
   powertrainModel.calculateWheelTorques(throttleActuation, torques);
 
   // Calculate dynamic states
-  Eigen::Vector3d dynamicStates = getDynamicStates(dt);
+  Eigen::Vector3d dynamicStates = getDynamicStates(dt, frictionCoefficients);
 
   // Update orientation based on angular velocity
-  orientation.z() += dt * angularVelocity.z();
+  orientation.z() += dt * angularVelocity.z() * 10;
 
   // Update acceleration from dynamic states
   acceleration = Eigen::Vector3d(dynamicStates.x(), dynamicStates.y(), 0.0);
diff --git a/src/pacsim/src/pacsim_main.cpp b/src/pacsim/src/pacsim_main.cpp
index 40f47aa0b..1a2a372cb 100644
--- a/src/pacsim/src/pacsim_main.cpp
+++ b/src/pacsim/src/pacsim_main.cpp
@@ -195,7 +195,7 @@ int threadMainLoopFunc(std::shared_ptr<rclcpp::Node> node)
         clockPub->publish(clockMsg);
         auto wheelPositions = model->getWheelPositions();
         Wheels gripValues = gm.getGripValues(wheelPositions);
-        model->forwardIntegrate(timestep/* , gripValues */);
+        model->forwardIntegrate(timestep, gripValues);
         auto t = model->getPosition();
         auto rEulerAngles = model->getOrientation();
         auto alpha = model->getAngularAcceleration();
diff --git a/src/planning/include/planning/planning.hpp b/src/planning/include/planning/planning.hpp
index b58505c51..2ecd3be21 100644
--- a/src/planning/include/planning/planning.hpp
+++ b/src/planning/include/planning/planning.hpp
@@ -16,6 +16,7 @@
 #include "custom_interfaces/msg/path_point.hpp"
 #include "custom_interfaces/msg/path_point_array.hpp"
 #include "custom_interfaces/msg/point2d.hpp"
+#include "custom_interfaces/msg/pose.hpp"
 #include "custom_interfaces/msg/point_array.hpp"
 #include "custom_interfaces/msg/vehicle_state.hpp"
 #include "planning/outliers.hpp"
@@ -73,7 +74,7 @@ class Planning : public rclcpp::Node {
   bool received_first_pose_ = false;
   std::vector<Cone> cone_array_;
   /**< Subscription to vehicle localization */
-  rclcpp::Subscription<custom_interfaces::msg::VehicleState>::SharedPtr vl_sub_;
+  rclcpp::Subscription<custom_interfaces::msg::Pose>::SharedPtr vl_sub_;
   /**< Subscription to track map */
   rclcpp::Subscription<custom_interfaces::msg::ConeArray>::SharedPtr track_sub_;
   /**< Local path points publisher */
@@ -90,7 +91,7 @@ class Planning : public rclcpp::Node {
    *
    * @param msg The received VehicleState message.
    */
-  void vehicle_localization_callback(const custom_interfaces::msg::VehicleState &msg);
+  void vehicle_localization_callback(const custom_interfaces::msg::Pose &msg);
 
   /**
    * @brief Fetches the mission from the parameters.
diff --git a/src/planning/src/adapter_planning/eufs.cpp b/src/planning/src/adapter_planning/eufs.cpp
index 589fcb974..a9ef29255 100644
--- a/src/planning/src/adapter_planning/eufs.cpp
+++ b/src/planning/src/adapter_planning/eufs.cpp
@@ -125,10 +125,10 @@ void EufsAdapter::set_mission_state(int mission, int state) {
 
 void EufsAdapter::pose_callback(const eufs_msgs::msg::CarState& msg) {
   RCLCPP_DEBUG(this->get_logger(), "Received pose from EUFS");
-  custom_interfaces::msg::VehicleState pose;
+  custom_interfaces::msg::Pose pose;
   // only gets the x, y, and theta since those are the only ones necessary for planning
-  pose.position.x = msg.pose.pose.position.x;
-  pose.position.y = msg.pose.pose.position.y;
+  pose.x = msg.pose.pose.position.x;
+  pose.y = msg.pose.pose.position.y;
   pose.theta = atan2(2.0f * (msg.pose.pose.orientation.w * msg.pose.pose.orientation.z +
                              msg.pose.pose.orientation.x * msg.pose.pose.orientation.y),
                      msg.pose.pose.orientation.w * msg.pose.pose.orientation.w +
diff --git a/src/planning/src/adapter_planning/fsds.cpp b/src/planning/src/adapter_planning/fsds.cpp
index 21af79bfc..cd7df3f2a 100644
--- a/src/planning/src/adapter_planning/fsds.cpp
+++ b/src/planning/src/adapter_planning/fsds.cpp
@@ -37,7 +37,7 @@ void FsdsAdapter::finish() {
 }
 
 void FsdsAdapter::pose_callback(const nav_msgs::msg::Odometry& msg) {
-  custom_interfaces::msg::VehicleState pose;
+  custom_interfaces::msg::Pose pose;
   pose.header = msg.header;
   tf2::Quaternion q(msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
                     msg.pose.pose.orientation.z, msg.pose.pose.orientation.w);
@@ -47,7 +47,7 @@ void FsdsAdapter::pose_callback(const nav_msgs::msg::Odometry& msg) {
   double yaw;
   m.getRPY(roll, pitch, yaw);
   pose.theta = yaw;
-  pose.position.x = msg.pose.pose.position.x;
-  pose.position.y = msg.pose.pose.position.y;
+  pose.x = msg.pose.pose.position.x;
+  pose.y = msg.pose.pose.position.y;
   this->vehicle_localization_callback(pose);
 }
diff --git a/src/planning/src/adapter_planning/pacsim.cpp b/src/planning/src/adapter_planning/pacsim.cpp
index 4a482af6d..5b48d4099 100644
--- a/src/planning/src/adapter_planning/pacsim.cpp
+++ b/src/planning/src/adapter_planning/pacsim.cpp
@@ -21,7 +21,7 @@ void PacSimAdapter::timer_callback() {
   RCLCPP_DEBUG(this->get_logger(), "Planning pacsim timer callback");
   if (tf_buffer_->canTransform("map", "car", tf2::TimePointZero)) {
     RCLCPP_DEBUG(this->get_logger(), "Planning recieved already recieved first pose\n");
-    custom_interfaces::msg::VehicleState pose;
+    custom_interfaces::msg::Pose pose;
     geometry_msgs::msg::TransformStamped t =
         tf_buffer_->lookupTransform("map", "car", tf2::TimePointZero);
     pose.header = t.header;
@@ -33,10 +33,8 @@ void PacSimAdapter::timer_callback() {
     double yaw;
     m.getRPY(roll, pitch, yaw);
     pose.theta = yaw;
-    pose.position.x = t.transform.translation.x;
-    pose.position.y = t.transform.translation.y;
-    pose.linear_velocity = 0.0;   // not needed -> default value
-    pose.angular_velocity = 0.0;  // not needed -> default value
+    pose.x = t.transform.translation.x;
+    pose.y = t.transform.translation.y;
     this->vehicle_localization_callback(pose);
   }
 }
diff --git a/src/planning/src/planning/planning.cpp b/src/planning/src/planning/planning.cpp
index 249c3026d..081f66051 100644
--- a/src/planning/src/planning/planning.cpp
+++ b/src/planning/src/planning/planning.cpp
@@ -95,8 +95,8 @@ Planning::Planning(const PlanningParameters &params)
 
   if (!planning_config_.simulation_.using_simulated_se_) {
     // Vehicle Localization Subscriber
-    this->vl_sub_ = this->create_subscription<custom_interfaces::msg::VehicleState>(
-        "/state_estimation/vehicle_state", 10,
+    this->vl_sub_ = this->create_subscription<custom_interfaces::msg::Pose>(
+        "/state_estimation/vehicle_pose", 10,
         std::bind(&Planning::vehicle_localization_callback, this, _1));
     // State Estimation map Subscriber
     this->track_sub_ = this->create_subscription<custom_interfaces::msg::ConeArray>(
@@ -216,8 +216,8 @@ void Planning::run_planning_algorithms() {
   }
 }
 
-void Planning::vehicle_localization_callback(const custom_interfaces::msg::VehicleState &msg) {
-  this->pose = Pose(msg.position.x, msg.position.y, msg.theta);
+void Planning::vehicle_localization_callback(const custom_interfaces::msg::Pose &msg) {
+  this->pose = Pose(msg.x, msg.y, msg.theta);
 
   if (!this->received_first_pose_) {
     this->initial_car_orientation_ = msg.theta;