diff --git a/cfg/ModelCarControl.cfg b/cfg/ModelCarControl.cfg
index c7138b3fe1a4177e47829f0b6f86fbd096ceb2d9..74abfafe3c4905c04f6bd65344a775cb6c4349bf 100755
--- a/cfg/ModelCarControl.cfg
+++ b/cfg/ModelCarControl.cfg
@@ -43,6 +43,7 @@ gen.add("speed_Kp",         double_t,  0,           "Proportional constant of th
 gen.add("speed_Ki",         double_t,  0,           "Integral constant of the speed PID",         0.0,    0.0, 1000.0)
 gen.add("speed_Kd",         double_t,  0,           "Differential constant of the speed PID",     0.0,    0.0, 1000.0)
 gen.add("speed_i_max",      double_t,  0,           "Maximum integral value pf the speed PID",    10.0,   0.0, 1000.0)
+gen.add("speed_deadband",   double_t,  0,           "Minimum valid speed command",                0.1,    0.1, 1.0)
 gen.add("watchdog_time",    double_t,  0,           "Maximum time between two control commands",  0.5,    0.0, 1000.0)
 gen.add("axel_distance",    double_t,  0,           "Distance between the two axels in meters",   0.3662, 0.0, 10.0)
 gen.add("max_steer_angle",  double_t,  0,           "Maximum steering angle",                     0.4,    -1.0,1.0)
diff --git a/config/car1_params.yaml b/config/car1_params.yaml
index a97da440fcada61154ebef6aba730546a6e4c565..ca611b78bceec41d8ea9e9dfec423ab724c41d65 100644
--- a/config/car1_params.yaml
+++ b/config/car1_params.yaml
@@ -1,8 +1,9 @@
 rate: 40
 speed_Kp: 20.0
-speed_Ki: 10.0
-speed_Kd: 0.05
+speed_Ki: 50.0
+speed_Kd: 0.00
 speed_i_max: 20.0
+speed_deadband: 0.1
 watchdog_time: 0.5
 axel_distance: 0.3662
 max_steer_angle: 0.4
diff --git a/config/car2_params.yaml b/config/car2_params.yaml
index a97da440fcada61154ebef6aba730546a6e4c565..ca611b78bceec41d8ea9e9dfec423ab724c41d65 100644
--- a/config/car2_params.yaml
+++ b/config/car2_params.yaml
@@ -1,8 +1,9 @@
 rate: 40
 speed_Kp: 20.0
-speed_Ki: 10.0
-speed_Kd: 0.05
+speed_Ki: 50.0
+speed_Kd: 0.00
 speed_i_max: 20.0
+speed_deadband: 0.1
 watchdog_time: 0.5
 axel_distance: 0.3662
 max_steer_angle: 0.4
diff --git a/config/params.yaml b/config/params.yaml
index a97da440fcada61154ebef6aba730546a6e4c565..ca611b78bceec41d8ea9e9dfec423ab724c41d65 100644
--- a/config/params.yaml
+++ b/config/params.yaml
@@ -1,8 +1,9 @@
 rate: 40
 speed_Kp: 20.0
-speed_Ki: 10.0
-speed_Kd: 0.05
+speed_Ki: 50.0
+speed_Kd: 0.00
 speed_i_max: 20.0
+speed_deadband: 0.1
 watchdog_time: 0.5
 axel_distance: 0.3662
 max_steer_angle: 0.4
diff --git a/src/model_car_control_alg_node.cpp b/src/model_car_control_alg_node.cpp
index 2441898e5129d55c3168a90f2e0e60c3f7e01112..a697e0aa939ba00b9c99a56ed19a541b130da1b9 100644
--- a/src/model_car_control_alg_node.cpp
+++ b/src/model_car_control_alg_node.cpp
@@ -76,7 +76,7 @@ void ModelCarControlAlgNode::mainNodeThread(void)
       }
       else
       {
-        if(this->linear_speed_control==0.0)
+        if(fabs(this->linear_speed_control)<this->config_.speed_deadband)
         {
           this->control_AckermannDriveStamped_msg_.drive.speed=0.0;
           this->speed_pid.reset();
@@ -92,7 +92,10 @@ void ModelCarControlAlgNode::mainNodeThread(void)
           else if(this->control_AckermannDriveStamped_msg_.drive.speed<this->config_.min_speed_control)
             this->control_AckermannDriveStamped_msg_.drive.speed=this->config_.min_speed_control;
           //angular speed
-          this->control_AckermannDriveStamped_msg_.drive.steering_angle=this->steer_angle_control*(this->config_.max_steer_control-this->config_.min_steer_control)/(this->config_.max_steer_angle-this->config_.min_steer_angle);
+          if(this->steer_angle_control>=0)
+            this->control_AckermannDriveStamped_msg_.drive.steering_angle=(this->steer_angle_control/this->config_.max_steer_angle)*this->config_.max_steer_control;
+          else 
+            this->control_AckermannDriveStamped_msg_.drive.steering_angle=(this->steer_angle_control/this->config_.min_steer_angle)*this->config_.min_steer_control;
         }
         last_time=this->feedback_time;
       }