Commande non linéaire de vitesse

workspace/devel/.private/detect_targets/include/detect_targets/TriangleParamConfig.h

@@ -249,6 +249,7 @@ class DEFAULT
         if("linear_x_Ki"==(*_i)->name){linear_x_Ki = boost::any_cast<double>(val);}
         if("linear_x_Kd"==(*_i)->name){linear_x_Kd = boost::any_cast<double>(val);}
         if("control_linear_x"==(*_i)->name){control_linear_x = boost::any_cast<bool>(val);}
+        if("speed_corrector_x"==(*_i)->name){speed_corrector_x = boost::any_cast<double>(val);}
         if("speed_linear_y_Kp"==(*_i)->name){speed_linear_y_Kp = boost::any_cast<double>(val);}
         if("speed_linear_y_Ki"==(*_i)->name){speed_linear_y_Ki = boost::any_cast<double>(val);}
         if("speed_linear_y_Kd"==(*_i)->name){speed_linear_y_Kd = boost::any_cast<double>(val);}
@@ -256,6 +257,7 @@ class DEFAULT
         if("linear_y_Ki"==(*_i)->name){linear_y_Ki = boost::any_cast<double>(val);}
         if("linear_y_Kd"==(*_i)->name){linear_y_Kd = boost::any_cast<double>(val);}
         if("control_linear_y"==(*_i)->name){control_linear_y = boost::any_cast<bool>(val);}
+        if("speed_corrector_y"==(*_i)->name){speed_corrector_y = boost::any_cast<double>(val);}
         if("linear_z_Kp"==(*_i)->name){linear_z_Kp = boost::any_cast<double>(val);}
         if("linear_z_Ki"==(*_i)->name){linear_z_Ki = boost::any_cast<double>(val);}
         if("linear_z_Kd"==(*_i)->name){linear_z_Kd = boost::any_cast<double>(val);}
@@ -280,6 +282,7 @@ double linear_x_Kp;
 double linear_x_Ki;
 double linear_x_Kd;
 bool control_linear_x;
+double speed_corrector_x;
 double speed_linear_y_Kp;
 double speed_linear_y_Ki;
 double speed_linear_y_Kd;
@@ -287,6 +290,7 @@ double linear_y_Kp;
 double linear_y_Ki;
 double linear_y_Kd;
 bool control_linear_y;
+double speed_corrector_y;
 double linear_z_Kp;
 double linear_z_Ki;
 double linear_z_Kd;
@@ -330,6 +334,8 @@ bool control_angular_z;
       double linear_x_Kd;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
       bool control_linear_x;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      double speed_corrector_x;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
       double speed_linear_y_Kp;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
@@ -344,6 +350,8 @@ bool control_angular_z;
       double linear_y_Kd;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
       bool control_linear_y;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      double speed_corrector_y;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
       double linear_z_Kp;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
@@ -541,7 +549,7 @@ TriangleParamConfig::GroupDescription<TriangleParamConfig::DEFAULT, TrianglePara
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
       __min__.max_speed = 0.01;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
-      __max__.max_speed = 1.0;
+      __max__.max_speed = 3.0;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
       __default__.max_speed = 0.3;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
@@ -551,7 +559,7 @@ TriangleParamConfig::GroupDescription<TriangleParamConfig::DEFAULT, TrianglePara
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
       __min__.max_acceleration = 0.01;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
-      __max__.max_acceleration = 1.0;
+      __max__.max_acceleration = 3.0;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
       __default__.max_acceleration = 0.3;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
@@ -628,6 +636,16 @@ TriangleParamConfig::GroupDescription<TriangleParamConfig::DEFAULT, TrianglePara
       Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<bool>("control_linear_x", "bool", 0, "Control distance to target", "", &TriangleParamConfig::control_linear_x)));
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
       __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<bool>("control_linear_x", "bool", 0, "Control distance to target", "", &TriangleParamConfig::control_linear_x)));
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __min__.speed_corrector_x = 0.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __max__.speed_corrector_x = 3.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __default__.speed_corrector_x = 0.1;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("speed_corrector_x", "double", 0, "Distance from which the speed corrector is disabled", "", &TriangleParamConfig::speed_corrector_x)));
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("speed_corrector_x", "double", 0, "Distance from which the speed corrector is disabled", "", &TriangleParamConfig::speed_corrector_x)));
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
       __min__.speed_linear_y_Kp = 0.0;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
@@ -698,6 +716,16 @@ TriangleParamConfig::GroupDescription<TriangleParamConfig::DEFAULT, TrianglePara
       Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<bool>("control_linear_y", "bool", 0, "Controls the facing to target", "", &TriangleParamConfig::control_linear_y)));
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
       __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<bool>("control_linear_y", "bool", 0, "Controls the facing to target", "", &TriangleParamConfig::control_linear_y)));
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __min__.speed_corrector_y = 0.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __max__.speed_corrector_y = 3.0;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __default__.speed_corrector_y = 0.1;
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      Default.abstract_parameters.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("speed_corrector_y", "double", 0, "Distance from which the speed corrector is disabled", "", &TriangleParamConfig::speed_corrector_y)));
+//#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
+      __param_descriptions__.push_back(TriangleParamConfig::AbstractParamDescriptionConstPtr(new TriangleParamConfig::ParamDescription<double>("speed_corrector_y", "double", 0, "Distance from which the speed corrector is disabled", "", &TriangleParamConfig::speed_corrector_y)));
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"
       __min__.linear_z_Kp = 0.0;
 //#line 291 "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py"

workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig-usage.dox

@@ -14,6 +14,7 @@
   <param name="linear_x_Ki" type="double" value="0.01" />
   <param name="linear_x_Kd" type="double" value="0.01" />
   <param name="control_linear_x" type="bool" value="True" />
+  <param name="speed_corrector_x" type="double" value="0.1" />
   <param name="speed_linear_y_Kp" type="double" value="0.01" />
   <param name="speed_linear_y_Ki" type="double" value="0.01" />
   <param name="speed_linear_y_Kd" type="double" value="0.01" />
@@ -21,6 +22,7 @@
   <param name="linear_y_Ki" type="double" value="0.01" />
   <param name="linear_y_Kd" type="double" value="0.01" />
   <param name="control_linear_y" type="bool" value="True" />
+  <param name="speed_corrector_y" type="double" value="0.1" />
   <param name="linear_z_Kp" type="double" value="0.01" />
   <param name="linear_z_Ki" type="double" value="0.01" />
   <param name="linear_z_Kd" type="double" value="0.01" />

workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.dox

@@ -6,8 +6,8 @@ Reads and maintains the following parameters on the ROS server
 - \b "~target_width" : \b [double] the real target width (m) min: 0.01, default: 1.0, max: 1.5
 - \b "~target_depth" : \b [double] the real target depth (m) min: 0.01, default: 0.2, max: 0.5
 - \b "~distance_to_target" : \b [double] The required distance to the target (m) min: 1.0, default: 2.0, max: 5.0
-- \b "~max_speed" : \b [double] the maximal linear speed min: 0.01, default: 0.3, max: 1.0
+- \b "~max_speed" : \b [double] the maximal linear speed min: 0.01, default: 0.3, max: 3.0
-- \b "~max_acceleration" : \b [double] the maximal linear speed min: 0.01, default: 0.3, max: 1.0
+- \b "~max_acceleration" : \b [double] the maximal linear speed min: 0.01, default: 0.3, max: 3.0
 - \b "~speed_linear_x_Kp" : \b [double] linear.x controller Kp min: 0.0, default: 0.01, max: 2.0
 - \b "~speed_linear_x_Ki" : \b [double] linear.x controller Ki min: 0.0, default: 0.01, max: 2.0
 - \b "~speed_linear_x_Kd" : \b [double] linear.x controller Kd min: 0.0, default: 0.01, max: 2.0
@@ -15,6 +15,7 @@ Reads and maintains the following parameters on the ROS server
 - \b "~linear_x_Ki" : \b [double] linear.x controller Ki min: 0.0, default: 0.01, max: 1.0
 - \b "~linear_x_Kd" : \b [double] linear.x controller Kd min: 0.0, default: 0.01, max: 1.0
 - \b "~control_linear_x" : \b [bool] Control distance to target min: False, default: True, max: True
+- \b "~speed_corrector_x" : \b [double] Distance from which the speed corrector is disabled min: 0.0, default: 0.1, max: 3.0
 - \b "~speed_linear_y_Kp" : \b [double] linear.y controller Kp min: 0.0, default: 0.01, max: 2.0
 - \b "~speed_linear_y_Ki" : \b [double] linear.y controller Ki min: 0.0, default: 0.01, max: 2.0
 - \b "~speed_linear_y_Kd" : \b [double] linear.y controller Kd min: 0.0, default: 0.01, max: 2.0
@@ -22,6 +23,7 @@ Reads and maintains the following parameters on the ROS server
 - \b "~linear_y_Ki" : \b [double] linear.y controller Ki min: 0.0, default: 0.01, max: 1.0
 - \b "~linear_y_Kd" : \b [double] linear.y controller Kd min: 0.0, default: 0.01, max: 1.0
 - \b "~control_linear_y" : \b [bool] Controls the facing to target min: False, default: True, max: True
+- \b "~speed_corrector_y" : \b [double] Distance from which the speed corrector is disabled min: 0.0, default: 0.1, max: 3.0
 - \b "~linear_z_Kp" : \b [double] linear.z controller Kp min: 0.0, default: 0.01, max: 10.0
 - \b "~linear_z_Ki" : \b [double] linear.z controller Ki min: 0.0, default: 0.01, max: 10.0
 - \b "~linear_z_Kd" : \b [double] linear.z controller Kd min: 0.0, default: 0.01, max: 10.0

workspace/devel/.private/detect_targets/share/detect_targets/docs/TriangleParamConfig.wikidoc

@@ -22,11 +22,11 @@ desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfig
 4.name= ~max_speed
 4.default= 0.3
 4.type= double
-4.desc=the maximal linear speed Range: 0.01 to 1.0
+4.desc=the maximal linear speed Range: 0.01 to 3.0
 5.name= ~max_acceleration
 5.default= 0.3
 5.type= double
-5.desc=the maximal linear speed Range: 0.01 to 1.0
+5.desc=the maximal linear speed Range: 0.01 to 3.0
 6.name= ~speed_linear_x_Kp
 6.default= 0.01
 6.type= double
@@ -55,66 +55,74 @@ desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfig
 12.default= True
 12.type= bool
 12.desc=Control distance to target 
-13.name= ~speed_linear_y_Kp
+13.name= ~speed_corrector_x
-13.default= 0.01
+13.default= 0.1
 13.type= double
-13.desc=linear.y controller Kp Range: 0.0 to 2.0
+13.desc=Distance from which the speed corrector is disabled Range: 0.0 to 3.0
-14.name= ~speed_linear_y_Ki
+14.name= ~speed_linear_y_Kp
 14.default= 0.01
 14.type= double
-14.desc=linear.y controller Ki Range: 0.0 to 2.0
+14.desc=linear.y controller Kp Range: 0.0 to 2.0
-15.name= ~speed_linear_y_Kd
+15.name= ~speed_linear_y_Ki
 15.default= 0.01
 15.type= double
-15.desc=linear.y controller Kd Range: 0.0 to 2.0
+15.desc=linear.y controller Ki Range: 0.0 to 2.0
-16.name= ~linear_y_Kp
+16.name= ~speed_linear_y_Kd
 16.default= 0.01
 16.type= double
-16.desc=linear.y controller Kp Range: 0.0 to 1.0
+16.desc=linear.y controller Kd Range: 0.0 to 2.0
-17.name= ~linear_y_Ki
+17.name= ~linear_y_Kp
 17.default= 0.01
 17.type= double
-17.desc=linear.y controller Ki Range: 0.0 to 1.0
+17.desc=linear.y controller Kp Range: 0.0 to 1.0
-18.name= ~linear_y_Kd
+18.name= ~linear_y_Ki
 18.default= 0.01
 18.type= double
-18.desc=linear.y controller Kd Range: 0.0 to 1.0
+18.desc=linear.y controller Ki Range: 0.0 to 1.0
-19.name= ~control_linear_y
+19.name= ~linear_y_Kd
-19.default= True
+19.default= 0.01
-19.type= bool
+19.type= double
-19.desc=Controls the facing to target 
+19.desc=linear.y controller Kd Range: 0.0 to 1.0
-20.name= ~linear_z_Kp
+20.name= ~control_linear_y
-20.default= 0.01
+20.default= True
-20.type= double
+20.type= bool
-20.desc=linear.z controller Kp Range: 0.0 to 10.0
+20.desc=Controls the facing to target 
-21.name= ~linear_z_Ki
+21.name= ~speed_corrector_y
-21.default= 0.01
+21.default= 0.1
 21.type= double
-21.desc=linear.z controller Ki Range: 0.0 to 10.0
+21.desc=Distance from which the speed corrector is disabled Range: 0.0 to 3.0
-22.name= ~linear_z_Kd
+22.name= ~linear_z_Kp
 22.default= 0.01
 22.type= double
-22.desc=linear.z controller Kd Range: 0.0 to 10.0
+22.desc=linear.z controller Kp Range: 0.0 to 10.0
-23.name= ~control_linear_z
+23.name= ~linear_z_Ki
-23.default= True
+23.default= 0.01
-23.type= bool
+23.type= double
-23.desc=Controls the facing to target 
+23.desc=linear.z controller Ki Range: 0.0 to 10.0
-24.name= ~angular_z_Kp
+24.name= ~linear_z_Kd
 24.default= 0.01
 24.type= double
-24.desc=angular.z controller Kp Range: 0.0 to 10.0
+24.desc=linear.z controller Kd Range: 0.0 to 10.0
-25.name= ~angular_z_Ki
+25.name= ~control_linear_z
-25.default= 0.01
+25.default= True
-25.type= double
+25.type= bool
-25.desc=angular.z controller Ki Range: 0.0 to 10.0
+25.desc=Controls the facing to target 
-26.name= ~angular_z_Kd
+26.name= ~angular_z_Kp
 26.default= 0.01
 26.type= double
-26.desc=angular.z controller Kd Range: 0.0 to 10.0
+26.desc=angular.z controller Kp Range: 0.0 to 10.0
-27.name= ~control_angular_z
+27.name= ~angular_z_Ki
-27.default= True
+27.default= 0.01
-27.type= bool
+27.type= double
-27.desc=Controls the facing to target 
+27.desc=angular.z controller Ki Range: 0.0 to 10.0
+28.name= ~angular_z_Kd
+28.default= 0.01
+28.type= double
+28.desc=angular.z controller Kd Range: 0.0 to 10.0
+29.name= ~control_angular_z
+29.default= True
+29.type= bool
+29.desc=Controls the facing to target 
 }
 }
 # End of autogenerated section. You may edit below.

workspace/src/detect_targets/cfg/triangle_control.cfg

@@ -9,8 +9,8 @@ gen.add("camera_angle", double_t, 0, "The angle corresponding to the image width
 gen.add("target_width", double_t, 0, "the real target width (m)", 1, 0.01, 1.5)
 gen.add("target_depth", double_t, 0, "the real target depth (m)", .2, 0.01, 0.5)
 gen.add("distance_to_target", double_t, 0, "The required distance to the target (m)", 2, 1, 5)
-gen.add("max_speed", double_t, 0, "the maximal linear speed", .3, .01, 1)
+gen.add("max_speed", double_t, 0, "the maximal linear speed", .3, .01, 3)
-gen.add("max_acceleration", double_t, 0, "the maximal linear speed", .3, .01, 1)
+gen.add("max_acceleration", double_t, 0, "the maximal linear speed", .3, .01, 3)
 
 
 gen.add("speed_linear_x_Kp", double_t, 0, "linear.x controller Kp", .01, 0, 2)
@@ -20,6 +20,7 @@ gen.add("linear_x_Kp", double_t, 0, "linear.x controller Kp", .01, 0, 1)
 gen.add("linear_x_Ki", double_t, 0, "linear.x controller Ki", .01, 0, 1)
 gen.add("linear_x_Kd", double_t, 0, "linear.x controller Kd", .01, 0, 1)
 gen.add("control_linear_x", bool_t, 0, "Control distance to target", True)
+gen.add("speed_corrector_x", double_t, 0, "Distance from which the speed corrector is disabled", .1, 0, 3)
 
 gen.add("speed_linear_y_Kp", double_t, 0, "linear.y controller Kp", .01, 0, 2)
 gen.add("speed_linear_y_Ki", double_t, 0, "linear.y controller Ki", .01, 0, 2)
@@ -28,6 +29,7 @@ gen.add("linear_y_Kp", double_t, 0, "linear.y controller Kp", .01, 0, 1)
 gen.add("linear_y_Ki", double_t, 0, "linear.y controller Ki", .01, 0, 1)
 gen.add("linear_y_Kd", double_t, 0, "linear.y controller Kd", .01, 0, 1)
 gen.add("control_linear_y", bool_t, 0, "Controls the facing to target", True)
+gen.add("speed_corrector_y", double_t, 0, "Distance from which the speed corrector is disabled", .1, 0, 3)
 
 gen.add("linear_z_Kp", double_t, 0, "linear.z controller Kp", .01, 0, 10)
 gen.add("linear_z_Ki", double_t, 0, "linear.z controller Ki", .01, 0, 10)

workspace/src/detect_targets/scripts/triangle_control.py

@@ -4,6 +4,8 @@
 import math
 import time
 
+import numpy as np
+
 import roslib
 import rospy
 from geometry_msgs.msg import Twist
@@ -67,6 +69,8 @@ class TriangleControl:
             -config['max_speed'],
             config['max_speed']
         )
+        self.speed_corrector_y = config['speed_corrector_y']
+
         # X gains are reversed because of the chosen axis
         self.pid_linear_x.Kp = - config['linear_x_Kp']
         self.pid_linear_x.Ki = - config['linear_x_Ki']
@@ -87,6 +91,7 @@ class TriangleControl:
             -config['max_speed'],
             config['max_speed']
         )
+        self.speed_corrector_x = config['speed_corrector_x']
         self.pid_linear_x.setpoint = self.target_distance
 
         return config
@@ -169,11 +174,15 @@ class TriangleControl:
             self.linear_z_pub.publish(self.linear_z_info)
 
         dt = time.time() - self.pid_linear_y._last_time
-        last_input_y = self.pid_linear_y._last_input
+        self.last_values_y = np.concatenate((self.last_values_y[1:7], [self.alpha]))
         target_acceleration_y = self.pid_linear_y(-self.alpha)
+        if abs(self.alpha) >= self.speed_corrector_y:
             self.pid_speed_linear_y.setpoint = target_acceleration_y
-        speed_y = (last_input_y - self.pid_linear_y._last_input) / dt
+            speed_y = self.last_values_y.dot(self.savgol_filter) / dt
             self.twist.linear.y = self.pid_speed_linear_y(speed_y)
+        else:
+            self.twist.linear.y = target_acceleration_y
+
         if self.linear_y_pub.get_num_connections() > 0:
             self.linear_y_info.target = 0
             self.linear_y_info.error = -self.alpha
@@ -182,15 +191,20 @@ class TriangleControl:
             self.linear_y_pub.publish(self.linear_y_info)
 
         dt = time.time() - self.pid_linear_x._last_time
-        last_input_x = self.pid_linear_x._last_input
+        self.last_values_x = np.concatenate((self.last_values_x[1:7], [self.d]))
         target_acceleration_x = self.pid_linear_x(self.d)
+        speed_x = 0
+        if abs(self.target_distance - self.d) >= self.speed_corrector_x:
             self.pid_speed_linear_x.setpoint = target_acceleration_x
-        speed_x = (last_input_x - self.pid_linear_x._last_input) / dt
+            speed_x = self.last_values_x.dot(self.savgol_filter) / dt
             self.twist.linear.x = self.pid_speed_linear_x(speed_x)
+        else:
+            self.twist.linear.x = target_acceleration_x
+
         if self.linear_x_pub.get_num_connections() > 0:
             self.linear_x_info.target = self.pid_linear_x.setpoint
             self.linear_x_info.error = self.target_distance - self.d
-            self.linear_x_info.derror = 0
+            self.linear_x_info.derror = speed_x
             self.linear_x_info.cmd_vel = self.twist.linear.x
             self.linear_x_pub.publish(self.linear_x_info)
 
@@ -259,6 +273,22 @@ class TriangleControl:
             sample_time=0.14
         )
 
+        self.savgol_filter = 1.0/252.0 * np.array([
+            [22],
+            [-67],
+            [-58],
+            [0],
+            [58],
+            [67],
+            [-22]
+        ], dtype=np.float64)
+
+        self.last_values_x = np.zeros(7, dtype=np.float64)
+        self.last_values_y = np.zeros(7, dtype=np.float64)
+
+        self.speed_corrector_x = 30
+        self.speed_corrector_y = 30
+
         # Control info
         self.angular_z_info = control()
         self.linear_x_info = control()