Debug des nœuds de rate et triangle

CMakeLists.txt

@@ -95,12 +95,13 @@ generate_messages(
 generate_dynamic_reconfigure_options(
   cfg/DetectTargets.cfg
   cfg/triangle_control.cfg
-  cfg/PIDNode.cfg
+  cfg/RateNode.cfg
   cfg/ProportionalNode.cfg
   cfg/IntegralNode.cfg
   cfg/DerivativeNode.cfg
   cfg/SaturateNode.cfg
   cfg/InputNode.cfg
+  cfg/Triangle.cfg
 )
 
 ###################################

cfg/Triangle.cfg

@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+PACKAGE = "detect_targets"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("camera_angle", double_t, 0, "The angle corresponding to the image width", 80, 50, 180)
+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)
+
+exit(gen.generate(PACKAGE, "detect_targets", "Triangle"))

launch/internal_tun.launch

@@ -1,6 +1,12 @@
 <launch>
   <remap from="/reset" to="/bebop/reset"/>
   <node name="reconf" pkg="rqt_reconfigure" type="rqt_reconfigure"/>
+  <node name="Ratex_param" pkg="dynamic_reconfigure" type="dynparam" args="load /ratex $(find detect_targets)/params/settings_test_D_rate.yaml"/>
+  <node name="Ratey_param" pkg="dynamic_reconfigure" type="dynparam" args="load /ratey $(find detect_targets)/params/settings_test_D_rate.yaml"/>
+  <node name="Dx_param" pkg="dynamic_reconfigure" type="dynparam" args="load /Dx $(find detect_targets)/params/settings_test_D.yaml"/>
+  <node name="Dy_param" pkg="dynamic_reconfigure" type="dynparam" args="load /Dy $(find detect_targets)/params/settings_test_D.yaml"/>
+  <node name="safe_param" pkg="dynamic_reconfigure" type="dynparam" args="load /safe $(find detect_targets)/params/settings_safe.yaml"/>
+  <node name="targets_param" pkg="dynamic_reconfigure" type="dynparam" args="load /targets $(find detect_targets)/params/settings_blue.yaml"/>
   <include file="$(find bebop_driver)/launch/bebop_node.launch" />
 
   <node name="targets" pkg="detect_targets" type="target_publisher.py">
@@ -13,25 +19,22 @@
   <node name="untwist" pkg="detect_targets" type="untwist.py" output="screen">
     <remap from="input" to="/bebop/cmd_vel" />
   </node>
-
+  <node name="ratex" pkg="detect_targets" type="control_compute.py" args="rate" output="screen">
-  <node name="Dx" pkg="detect_targets" type="control_compute.py" args="derivative" output="screen">
     <remap from="input" to="linear_x" />
+    <remap from="output" to="derivative_x_input" />
+  </node>
+  <node name="Dx" pkg="detect_targets" type="control_compute.py" args="derivative" output="screen">
+    <remap from="input" to="derivative_x_input" />
     <remap from="output" to="measure_x" />
   </node>
-  <node name="Dy" pkg="detect_targets" type="control_compute.py" args="derivative" output="screen">
+  <node name="ratey" pkg="detect_targets" type="control_compute.py" args="rate" output="screen">
     <remap from="input" to="linear_y" />
+    <remap from="output" to="derivative_y_input" />
+  </node>
+  <node name="Dy" pkg="detect_targets" type="control_compute.py" args="derivative" output="screen">
+    <remap from="input" to="derivative_y_input" />
     <remap from="output" to="measure_y" />
   </node>
-  <include file="$(find detect_targets)/launch/control.launch" ns="controller_angular_z">
-      <arg name="reset" value="/reset" />
-      <arg name="measure" value="/angular_z" />
-  </include>
-    <node name="twister" pkg="detect_targets" type="twist_controls.py">
-      <remap from="control_linear_z" to="controller_linear_z/output" />
-      <remap from="control_angular_z" to="controller_angular_z/output" />
-      <remap from="control_linear_x" to="controller_linear_x/internal_loop/output" />
-      <remap from="control_linear_y" to="controller_linear_y/internal_loop/output" />
-    </node>
 
   <node name="safe" pkg="demo_teleop" type="safe_drone_teleop.py" output="screen" launch-prefix="xterm -e">
     <remap from="takeoff" to="/bebop/takeoff"/>
@@ -41,7 +44,11 @@
   </node>
 
   <node name="view" pkg="rqt_image_view" type="rqt_image_view" args="/bebop/image_raw"/>
-  <node name="view__targets" pkg="rqt_image_view" type="rqt_image_view" args="/img_targets"/>
+  <node name="view_targets" pkg="rqt_image_view" type="rqt_image_view" args="/img_targets"/>
+
+  <node name="plot" pkg="rqt_plot" type="rqt_plot" args="/measure_x /measure_linear_x">
+
+  </node>
 
   <node name="graph" pkg="rqt_graph" type="rqt_graph" output="screen"></node>
 </launch>

launch/test_derivative.launch

@@ -1,7 +1,14 @@
 <launch>
   <node name="reconf" pkg="rqt_reconfigure" type="rqt_reconfigure"/>
+  <node name="D_param" pkg="dynamic_reconfigure" type="dynparam" args="load /D $(find detect_targets)/params/settings_test_D.yaml"/>
+  <node name="Rate_param" pkg="dynamic_reconfigure" type="dynparam" args="load /rate $(find detect_targets)/params/settings_test_D_rate.yaml"/>
 
-  <node name="publisher" pkg="detect_targets" type="publish_csv.py" args="$(find detect_targets)/walk.csv" output="screen">
+  <node name="publisher" pkg="detect_targets" type="publish_csv.py" args="$(find detect_targets)/utils/data/walk.csv" output="screen">
+    <remap from="output" to="rate_input" />
+  </node>
+
+  <node name="rate" pkg="detect_targets" type="control_compute.py" args="rate" output="screen">
+    <remap from="input" to="rate_input" />
     <remap from="output" to="derivative_input" />
   </node>
 

params/settings_blue.yaml

@@ -0,0 +1,31 @@
+!!python/object/new:dynamic_reconfigure.encoding.Config
+dictitems:
+  binary: true
+  blue_max: 255
+  blue_min: 24
+  green_max: 203
+  green_min: 91
+  groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+    dictitems:
+      binary: true
+      blue_max: 255
+      blue_min: 24
+      green_max: 203
+      green_min: 91
+      groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      id: 0
+      name: Default
+      parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      parent: 0
+      red_max: 75
+      red_min: 36
+      state: true
+      targets: true
+      type: ''
+    state: []
+  red_max: 75
+  red_min: 36
+  targets: true
+state: []

params/settings_safe.yaml

@@ -0,0 +1,21 @@
+!!python/object/new:dynamic_reconfigure.encoding.Config
+dictitems:
+  angular: 0.5
+  delay: 2.0
+  groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+    dictitems:
+      angular: 0.5
+      delay: 2.0
+      groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      id: 0
+      linear: 0.05
+      name: Default
+      parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      parent: 0
+      state: true
+      type: ''
+    state: []
+  linear: 0.05
+state: []

params/settings_targets.yaml

@@ -0,0 +1,31 @@
+!!python/object/new:dynamic_reconfigure.encoding.Config
+dictitems:
+  binary: false
+  blue_max: 255
+  blue_min: 136
+  green_max: 182
+  green_min: 65
+  groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+    dictitems:
+      binary: false
+      blue_max: 255
+      blue_min: 136
+      green_max: 182
+      green_min: 65
+      groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      id: 0
+      name: Default
+      parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      parent: 0
+      red_max: 127
+      red_min: 44
+      state: true
+      targets: false
+      type: ''
+    state: []
+  red_max: 127
+  red_min: 44
+  targets: false
+state: []

params/settings_test_D.yaml

@@ -0,0 +1,25 @@
+!!python/object/new:dynamic_reconfigure.encoding.Config
+dictitems:
+  deriv: 1
+  groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+    dictitems:
+      deriv: 1
+      groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      id: 0
+      k: 1.0
+      name: Default
+      parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      parent: 0
+      poly_order: 2
+      refresh_time: 0.05
+      size: 19
+      state: true
+      type: ''
+    state: []
+  k: 1.0
+  poly_order: 2
+  refresh_time: 0.05
+  size: 19
+state: []

params/settings_test_D_rate.yaml

@@ -0,0 +1,19 @@
+!!python/object/new:dynamic_reconfigure.encoding.Config
+dictitems:
+  groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+    dictitems:
+      groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      id: 0
+      k: 1.0
+      name: Default
+      parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      parent: 0
+      refresh_time: 0.05
+      state: true
+      type: ''
+    state: []
+  k: 1.0
+  refresh_time: 0.05
+state: []

params/settings_yellow.yaml

@@ -0,0 +1,31 @@
+!!python/object/new:dynamic_reconfigure.encoding.Config
+dictitems:
+  binary: false
+  blue_max: 92
+  blue_min: 30
+  green_max: 201
+  green_min: 109
+  groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+    dictitems:
+      binary: false
+      blue_max: 92
+      blue_min: 30
+      green_max: 201
+      green_min: 109
+      groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      id: 0
+      name: Default
+      parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      parent: 0
+      red_max: 186
+      red_min: 130
+      state: true
+      targets: false
+      type: ''
+    state: []
+  red_max: 186
+  red_min: 130
+  targets: false
+state: []

scripts/control_compute.py

@@ -11,12 +11,12 @@ from std_msgs.msg import Float64, Empty
 
 import dynamic_reconfigure.server
 from detect_targets.cfg import ProportionalNodeConfig, IntegralNodeConfig
-from detect_targets.cfg import DerivativeNodeConfig, InputNodeConfig,
+from detect_targets.cfg import DerivativeNodeConfig, InputNodeConfig
-from detect_targets.cfg import SaturateNodeConfig, RateNodeConfig       
+from detect_targets.cfg import SaturateNodeConfig, RateNodeConfig
 
 
 class ControlNode(object):
-    def __init__(self, refresh_time=1.0, config_class=None):
+    def __init__(self, refresh_time=0.05, config_class=None):
         self.last_time = rospy.get_rostime()
         self.refresh_time = rospy.Duration.from_sec(refresh_time)
         self.output = 0.0
@@ -59,31 +59,30 @@ class InputControlNode(ControlNode):
         self.input_topics["input"] = rospy.Subscriber("input", Float64, self.on_compute)
 
 class RateNode(InputControlNode):
-    def __init__(self, *args, **kwargs):
+    def __init__(self, refresh_time=0.05):
-        self.rate = rospy.Rate(20)
-        self.last_input = 0
         self.lock = threading.Lock()
-        super(RateNode, self).__init__(*args, **kwargs)
+        super(RateNode, self).__init__(refresh_time, RateNodeConfig)
+        self.rate = rospy.Rate(1.0/self.refresh_time.to_sec())
 
     def on_reconf(self, config, level):
-        c = super(SaturateNode, self).on_reconf(config, level)
+        c = super(RateNode, self).on_reconf(config, level)
-        self.rate = rospy.Rate(self.refresh_time)
+        self.rate = rospy.Rate(self.refresh_time.to_sec())
         return c
 
     def on_compute(self, value):
         self.lock.acquire()
-        self.last_input = value.data
+        self.output = value.data
         self.lock.release()
 
     def on_mainloop(self):
         while not rospy.is_shutdown():
             self.lock.acquire()
-            self.output.publish(self.last_input)
+            self.output_topic.publish(self.output)
             self.lock.release()
             self.rate.sleep()
 
 class ProportionalNode(InputControlNode):
-    def __init__(self, k=1.0, refresh_time=1.0):
+    def __init__(self, k=1.0, refresh_time=0.05):
         super(ProportionalNode, self).__init__(refresh_time, ProportionalNodeConfig)
         self.k = k
 
@@ -98,7 +97,7 @@ class ProportionalNode(InputControlNode):
             super(ProportionalNode, self).on_compute(value)
 
 class SaturateNode(InputControlNode):
-    def __init__(self, min=None, max=None, refresh_time=1.0, config=SaturateNodeConfig):
+    def __init__(self, min=None, max=None, refresh_time=0.05, config=SaturateNodeConfig):
         super(SaturateNode, self).__init__(refresh_time, config)
         self.min = min
         self.max = max
@@ -116,7 +115,7 @@ class SaturateNode(InputControlNode):
             super(SaturateNode, self).on_compute(value)
 
 class IntegralNode(SaturateNode):
-    def __init__(self, k=1.0, min=None, max=None, refresh_time=1.0):
+    def __init__(self, k=1.0, min=None, max=None, refresh_time=0.05):
         super(IntegralNode, self).__init__(min, max, refresh_time, IntegralNodeConfig)
         self.k = k
 
@@ -131,7 +130,7 @@ class IntegralNode(SaturateNode):
             return super(IntegralNode, self).on_compute(value)
 
 class DerivativeNode(InputControlNode):
-    def __init__(self, k=0.05, size=9, polyorder=3, deriv=1, refresh_time=1.0):
+    def __init__(self, k=0.05, size=9, polyorder=3, deriv=1, refresh_time=0.05):
         super(DerivativeNode, self).__init__(refresh_time, DerivativeNodeConfig)
         self.k = k
         self.filter = savgol_coeffs(size, polyorder, deriv, delta=refresh_time, use='dot')
@@ -183,7 +182,7 @@ class DifferenciateNode(InputControlNode):
 
 class InputNode(InputControlNode):
     def __init__(self):
-        super(InputNode, self).__init__(refresh_time=1.0, config_class=InputNodeConfig)
+        super(InputNode, self).__init__(refresh_time=0.05, config_class=InputNodeConfig)
 
     def on_reconf(self, config, level):
         self.output = config["value"]
@@ -196,7 +195,7 @@ class InputNode(InputControlNode):
 
 class SumNode(ControlNode):
     def __init__(self, nb_topics):
-        super(SumNode, self).__init__(refresh_time=1.0)
+        super(SumNode, self).__init__(refresh_time=0.05)
         self.nb_topics = int(nb_topics)
         self.inputs = dict()
         for i in range(self.nb_topics):
@@ -230,4 +229,4 @@ if __name__ == '__main__':
     else:
         node = node_class()
 
-    node.on_mailoop()
+    node.on_mainloop()

scripts/publish_csv.py

@@ -11,6 +11,7 @@ if __name__ == '__main__':
     rospy.init_node('publish_csv', anonymous=True)
     pub = rospy.Publisher('output', Float64, queue_size=1)
     file = sys.argv[1]
+    rospy.sleep(10.)
     with open(file) as csvfile:
         reader = csv.reader(csvfile)
         r = rospy.Rate(20)

scripts/triangle.py

@@ -94,7 +94,7 @@ class Triangle:
             "component_centers", component_centers, self.on_comp, queue_size=1)
 
         self.config_srv = dynamic_reconfigure.server.Server(
-            TriangleParamConfig, self.on_reconf)
+            TriangleConfig, self.on_reconf)
 
         self.br = tf.TransformBroadcaster()
 

utils/results/mesure_vitesse_big_quadra.eps

@@ -1,6 +1,6 @@
 %!PS-Adobe-3.0 EPSF-3.0
 %%Creator: matplotlib version 2.2.4, http://matplotlib.org/
-%%CreationDate: Sun Jun  2 18:28:50 2019
+%%CreationDate: Sun Jun  2 19:58:19 2019
 %%Orientation: portrait
 %%BoundingBox: -54 180 666 612
 %%EndComments

utils/results/mesure_vitesse_cubique.eps

@@ -1,6 +1,6 @@
 %!PS-Adobe-3.0 EPSF-3.0
 %%Creator: matplotlib version 2.2.4, http://matplotlib.org/
-%%CreationDate: Sun Jun  2 18:28:49 2019
+%%CreationDate: Sun Jun  2 19:58:18 2019
 %%Orientation: portrait
 %%BoundingBox: -54 180 666 612
 %%EndComments

utils/results/mesure_vitesse_quadratique.eps

@@ -1,6 +1,6 @@
 %!PS-Adobe-3.0 EPSF-3.0
 %%Creator: matplotlib version 2.2.4, http://matplotlib.org/
-%%CreationDate: Sun Jun  2 18:28:49 2019
+%%CreationDate: Sun Jun  2 19:58:18 2019
 %%Orientation: portrait
 %%BoundingBox: -54 180 666 612
 %%EndComments

utils/results/savgol_bode_plop.eps

@@ -0,0 +1,833 @@
+%!PS-Adobe-3.0 EPSF-3.0
+%%Creator: matplotlib version 2.2.4, http://matplotlib.org/
+%%CreationDate: Tue Jun  4 14:06:46 2019
+%%Orientation: portrait
+%%BoundingBox: 90 252 522 540
+%%EndComments
+%%BeginProlog
+/mpldict 8 dict def
+mpldict begin
+/m { moveto } bind def
+/l { lineto } bind def
+/r { rlineto } bind def
+/c { curveto } bind def
+/cl { closepath } bind def
+/box {
+m
+1 index 0 r
+0 exch r
+neg 0 r
+cl
+} bind def
+/clipbox {
+box
+clip
+newpath
+} bind def
+%!PS-Adobe-3.0 Resource-Font
+%%Title: DejaVu Sans
+%%Copyright: Copyright (c) 2003 by Bitstream, Inc. All Rights Reserved. Copyright (c) 2006 by Tavmjong Bah. All Rights Reserved. DejaVu changes are in public domain 
+%%Creator: Converted from TrueType to type 3 by PPR
+25 dict begin
+/_d{bind def}bind def
+/_m{moveto}_d
+/_l{lineto}_d
+/_cl{closepath eofill}_d
+/_c{curveto}_d
+/_sc{7 -1 roll{setcachedevice}{pop pop pop pop pop pop}ifelse}_d
+/_e{exec}_d
+/FontName /DejaVuSans def
+/PaintType 0 def
+/FontMatrix[.001 0 0 .001 0 0]def
+/FontBBox[-1021 -463 1793 1232]def
+/FontType 3 def
+/Encoding [ /hyphen /period /zero /one /two /five /seven /a /g /l /m /o /p ] def
+/FontInfo 10 dict dup begin
+/FamilyName (DejaVu Sans) def
+/FullName (DejaVu Sans) def
+/Notice (Copyright (c) 2003 by Bitstream, Inc. All Rights Reserved. Copyright (c) 2006 by Tavmjong Bah. All Rights Reserved. DejaVu changes are in public domain ) def
+/Weight (Book) def
+/Version (Version 2.35) def
+/ItalicAngle 0.0 def
+/isFixedPitch false def
+/UnderlinePosition -130 def
+/UnderlineThickness 90 def
+end readonly def
+/CharStrings 14 dict dup begin
+/.notdef 0 def
+/hyphen{361 0 49 234 312 314 _sc
+49 314 _m
+312 314 _l
+312 234 _l
+49 234 _l
+49 314 _l
+_cl}_d
+/period{318 0 107 0 210 124 _sc
+107 124 _m
+210 124 _l
+210 0 _l
+107 0 _l
+107 124 _l
+_cl}_d
+/zero{636 0 66 -13 570 742 _sc
+318 664 _m
+267 664 229 639 203 589 _c
+177 539 165 464 165 364 _c
+165 264 177 189 203 139 _c
+229 89 267 64 318 64 _c
+369 64 407 89 433 139 _c
+458 189 471 264 471 364 _c
+471 464 458 539 433 589 _c
+407 639 369 664 318 664 _c
+318 742 _m
+399 742 461 709 505 645 _c
+548 580 570 486 570 364 _c
+570 241 548 147 505 83 _c
+461 19 399 -13 318 -13 _c
+236 -13 173 19 130 83 _c
+87 147 66 241 66 364 _c
+66 486 87 580 130 645 _c
+173 709 236 742 318 742 _c
+_cl}_d
+/one{636 0 110 0 544 729 _sc
+124 83 _m
+285 83 _l
+285 639 _l
+110 604 _l
+110 694 _l
+284 729 _l
+383 729 _l
+383 83 _l
+544 83 _l
+544 0 _l
+124 0 _l
+124 83 _l
+_cl}_d
+/two{{636 0 73 0 536 742 _sc
+192 83 _m
+536 83 _l
+536 0 _l
+73 0 _l
+73 83 _l
+110 121 161 173 226 239 _c
+290 304 331 346 348 365 _c
+380 400 402 430 414 455 _c
+426 479 433 504 433 528 _c
+433 566 419 598 392 622 _c
+365 646 330 659 286 659 _c
+255 659 222 653 188 643 _c
+154 632 117 616 78 594 _c
+78 694 _l
+118 710 155 722 189 730 _c
+223 738 255 742 284 742 _c
+}_e{359 742 419 723 464 685 _c
+509 647 532 597 532 534 _c
+532 504 526 475 515 449 _c
+504 422 484 390 454 354 _c
+446 344 420 317 376 272 _c
+332 227 271 164 192 83 _c
+_cl}_e}_d
+/five{{636 0 77 -13 549 729 _sc
+108 729 _m
+495 729 _l
+495 646 _l
+198 646 _l
+198 467 _l
+212 472 227 476 241 478 _c
+255 480 270 482 284 482 _c
+365 482 429 459 477 415 _c
+525 370 549 310 549 234 _c
+549 155 524 94 475 51 _c
+426 8 357 -13 269 -13 _c
+238 -13 207 -10 175 -6 _c
+143 -1 111 6 77 17 _c
+77 116 _l
+106 100 136 88 168 80 _c
+199 72 232 69 267 69 _c
+}_e{323 69 368 83 401 113 _c
+433 143 450 183 450 234 _c
+450 284 433 324 401 354 _c
+368 384 323 399 267 399 _c
+241 399 214 396 188 390 _c
+162 384 135 375 108 363 _c
+108 729 _l
+_cl}_e}_d
+/seven{636 0 82 0 551 729 _sc
+82 729 _m
+551 729 _l
+551 687 _l
+286 0 _l
+183 0 _l
+432 646 _l
+82 646 _l
+82 729 _l
+_cl}_d
+/a{{613 0 60 -13 522 560 _sc
+343 275 _m
+270 275 220 266 192 250 _c
+164 233 150 205 150 165 _c
+150 133 160 107 181 89 _c
+202 70 231 61 267 61 _c
+317 61 357 78 387 114 _c
+417 149 432 196 432 255 _c
+432 275 _l
+343 275 _l
+522 312 _m
+522 0 _l
+432 0 _l
+432 83 _l
+411 49 385 25 355 10 _c
+325 -5 287 -13 243 -13 _c
+187 -13 142 2 109 33 _c
+76 64 60 106 60 159 _c
+}_e{60 220 80 266 122 298 _c
+163 329 224 345 306 345 _c
+432 345 _l
+432 354 _l
+432 395 418 427 391 450 _c
+364 472 326 484 277 484 _c
+245 484 215 480 185 472 _c
+155 464 127 453 100 439 _c
+100 522 _l
+132 534 164 544 195 550 _c
+226 556 256 560 286 560 _c
+365 560 424 539 463 498 _c
+502 457 522 395 522 312 _c
+_cl}_e}_d
+/g{{635 0 55 -207 544 560 _sc
+454 280 _m
+454 344 440 395 414 431 _c
+387 467 349 485 301 485 _c
+253 485 215 467 188 431 _c
+161 395 148 344 148 280 _c
+148 215 161 165 188 129 _c
+215 93 253 75 301 75 _c
+349 75 387 93 414 129 _c
+440 165 454 215 454 280 _c
+544 68 _m
+544 -24 523 -93 482 -139 _c
+440 -184 377 -207 292 -207 _c
+260 -207 231 -204 203 -200 _c
+175 -195 147 -188 121 -178 _c
+}_e{121 -91 _l
+147 -105 173 -115 199 -122 _c
+225 -129 251 -133 278 -133 _c
+336 -133 380 -117 410 -87 _c
+439 -56 454 -10 454 52 _c
+454 96 _l
+435 64 411 40 382 24 _c
+353 8 319 0 279 0 _c
+211 0 157 25 116 76 _c
+75 127 55 195 55 280 _c
+55 364 75 432 116 483 _c
+157 534 211 560 279 560 _c
+319 560 353 552 382 536 _c
+411 520 435 496 454 464 _c
+454 547 _l
+544 547 _l
+}_e{544 68 _l
+_cl}_e}_d
+/l{278 0 94 0 184 760 _sc
+94 760 _m
+184 760 _l
+184 0 _l
+94 0 _l
+94 760 _l
+_cl}_d
+/m{{974 0 91 0 889 560 _sc
+520 442 _m
+542 482 569 511 600 531 _c
+631 550 668 560 711 560 _c
+767 560 811 540 842 500 _c
+873 460 889 403 889 330 _c
+889 0 _l
+799 0 _l
+799 327 _l
+799 379 789 418 771 444 _c
+752 469 724 482 686 482 _c
+639 482 602 466 575 435 _c
+548 404 535 362 535 309 _c
+535 0 _l
+445 0 _l
+445 327 _l
+445 379 435 418 417 444 _c
+398 469 369 482 331 482 _c
+}_e{285 482 248 466 221 435 _c
+194 404 181 362 181 309 _c
+181 0 _l
+91 0 _l
+91 547 _l
+181 547 _l
+181 462 _l
+201 495 226 520 255 536 _c
+283 552 317 560 357 560 _c
+397 560 430 550 458 530 _c
+486 510 506 480 520 442 _c
+_cl}_e}_d
+/o{612 0 55 -13 557 560 _sc
+306 484 _m
+258 484 220 465 192 427 _c
+164 389 150 338 150 273 _c
+150 207 163 156 191 118 _c
+219 80 257 62 306 62 _c
+354 62 392 80 420 118 _c
+448 156 462 207 462 273 _c
+462 337 448 389 420 427 _c
+392 465 354 484 306 484 _c
+306 560 _m
+384 560 445 534 490 484 _c
+534 433 557 363 557 273 _c
+557 183 534 113 490 63 _c
+445 12 384 -13 306 -13 _c
+227 -13 165 12 121 63 _c
+77 113 55 183 55 273 _c
+55 363 77 433 121 484 _c
+165 534 227 560 306 560 _c
+_cl}_d
+/p{{635 0 91 -207 580 560 _sc
+181 82 _m
+181 -207 _l
+91 -207 _l
+91 547 _l
+181 547 _l
+181 464 _l
+199 496 223 520 252 536 _c
+281 552 316 560 356 560 _c
+422 560 476 533 518 481 _c
+559 428 580 359 580 273 _c
+580 187 559 117 518 65 _c
+476 13 422 -13 356 -13 _c
+316 -13 281 -5 252 10 _c
+223 25 199 49 181 82 _c
+487 273 _m
+487 339 473 390 446 428 _c
+418 466 381 485 334 485 _c
+}_e{286 485 249 466 222 428 _c
+194 390 181 339 181 273 _c
+181 207 194 155 222 117 _c
+249 79 286 61 334 61 _c
+381 61 418 79 446 117 _c
+473 155 487 207 487 273 _c
+_cl}_e}_d
+end readonly def
+
+/BuildGlyph
+ {exch begin
+ CharStrings exch
+ 2 copy known not{pop /.notdef}if
+ true 3 1 roll get exec
+ end}_d
+
+/BuildChar {
+ 1 index /Encoding get exch get
+ 1 index /BuildGlyph get exec
+}_d
+
+FontName currentdict end definefont pop
+end
+%%EndProlog
+mpldict begin
+90 252 translate
+432 288 0 0 clipbox
+gsave
+0 0 m
+432 0 l
+432 288 l
+0 288 l
+cl
+1.000 setgray
+fill
+grestore
+gsave
+12.904646 12.094646 m
+429.165354 12.094646 l
+429.165354 269.805354 l
+12.904646 269.805354 l
+cl
+1.000 setgray
+fill
+grestore
+0.500 setlinewidth
+1 setlinejoin
+2 setlinecap
+[] 0 setdash
+0.000 setgray
+gsave
+416.3 257.7 12.9 12.09 clipbox
+20.718955 12.094646 m
+20.718955 269.805354 l
+stroke
+grestore
+0.800 setlinewidth
+0 setlinecap
+gsave
+/o {
+gsave
+newpath
+translate
+0.8 setlinewidth
+1 setlinejoin
+0 setlinecap
+0 0 m
+0 3.5 l
+
+gsave
+0.000 setgray
+fill
+grestore
+stroke
+grestore
+} bind def
+20.719 12.0946 o
+grestore
+/DejaVuSans findfont
+8.000 scalefont
+setfont
+gsave
+14.351767 2.516521 translate
+0.000000 rotate
+0.000000 0.000000 m /zero glyphshow
+5.089844 0.000000 m /period glyphshow
+7.632812 0.000000 m /zero glyphshow
+grestore
+0.500 setlinewidth
+2 setlinecap
+gsave
+416.3 257.7 12.9 12.09 clipbox
+119.885314 12.094646 m
+119.885314 269.805354 l
+stroke
+grestore
+0.800 setlinewidth
+0 setlinecap
+gsave
+/o {
+gsave
+newpath
+translate
+0.8 setlinewidth
+1 setlinejoin
+0 setlinecap
+0 0 m
+0 3.5 l
+
+gsave
+0.000 setgray
+fill
+grestore
+stroke
+grestore
+} bind def
+119.885 12.0946 o
+grestore
+gsave
+113.518126 2.516521 translate
+0.000000 rotate
+0.000000 0.000000 m /two glyphshow
+5.089844 0.000000 m /period glyphshow
+7.632812 0.000000 m /five glyphshow
+grestore
+0.500 setlinewidth
+2 setlinecap
+gsave
+416.3 257.7 12.9 12.09 clipbox
+219.051673 12.094646 m
+219.051673 269.805354 l
+stroke
+grestore
+0.800 setlinewidth
+0 setlinecap
+gsave
+/o {
+gsave
+newpath
+translate
+0.8 setlinewidth
+1 setlinejoin
+0 setlinecap
+0 0 m
+0 3.5 l
+
+gsave
+0.000 setgray
+fill
+grestore
+stroke
+grestore
+} bind def
+219.052 12.0946 o
+grestore
+gsave
+212.684485 2.516521 translate
+0.000000 rotate
+0.000000 0.000000 m /five glyphshow
+5.089844 0.000000 m /period glyphshow
+7.632812 0.000000 m /zero glyphshow
+grestore
+0.500 setlinewidth
+2 setlinecap
+gsave
+416.3 257.7 12.9 12.09 clipbox
+318.218032 12.094646 m
+318.218032 269.805354 l
+stroke
+grestore
+0.800 setlinewidth
+0 setlinecap
+gsave
+/o {
+gsave
+newpath
+translate
+0.8 setlinewidth
+1 setlinejoin
+0 setlinecap
+0 0 m
+0 3.5 l
+
+gsave
+0.000 setgray
+fill
+grestore
+stroke
+grestore
+} bind def
+318.218 12.0946 o
+grestore
+gsave
+311.850844 2.516521 translate
+0.000000 rotate
+0.000000 0.000000 m /seven glyphshow
+5.089844 0.000000 m /period glyphshow
+7.632812 0.000000 m /five glyphshow
+grestore
+0.500 setlinewidth
+2 setlinecap
+gsave
+416.3 257.7 12.9 12.09 clipbox
+417.384391 12.094646 m
+417.384391 269.805354 l
+stroke
+grestore
+0.800 setlinewidth
+0 setlinecap
+gsave
+/o {
+gsave
+newpath
+translate
+0.8 setlinewidth
+1 setlinejoin
+0 setlinecap
+0 0 m
+0 3.5 l
+
+gsave
+0.000 setgray
+fill
+grestore
+stroke
+grestore
+} bind def
+417.384 12.0946 o
+grestore
+gsave
+408.470328 2.516521 translate
+0.000000 rotate
+0.000000 0.000000 m /one glyphshow
+5.089844 0.000000 m /zero glyphshow
+10.179688 0.000000 m /period glyphshow
+12.722656 0.000000 m /zero glyphshow
+grestore
+0.500 setlinewidth
+2 setlinecap
+gsave
+416.3 257.7 12.9 12.09 clipbox
+12.904646 40.948075 m
+429.165354 40.948075 l
+stroke
+grestore
+0.800 setlinewidth
+0 setlinecap
+gsave
+/o {
+gsave
+newpath
+translate
+0.8 setlinewidth
+1 setlinejoin
+0 setlinecap
+0 0 m
+3.5 0 l
+
+gsave
+0.000 setgray
+fill
+grestore
+stroke
+grestore
+} bind def
+12.9046 40.9481 o
+grestore
+gsave
+1.420271 37.909013 translate
+0.000000 rotate
+0.000000 0.000000 m /hyphen glyphshow
+2.886719 0.000000 m /two glyphshow
+grestore
+0.500 setlinewidth
+2 setlinecap
+gsave
+416.3 257.7 12.9 12.09 clipbox
+12.904646 112.156268 m
+429.165354 112.156268 l
+stroke
+grestore
+0.800 setlinewidth
+0 setlinecap
+gsave
+/o {
+gsave
+newpath
+translate
+0.8 setlinewidth
+1 setlinejoin
+0 setlinecap
+0 0 m
+3.5 0 l
+
+gsave
+0.000 setgray
+fill
+grestore
+stroke
+grestore
+} bind def
+12.9046 112.156 o
+grestore
+gsave
+1.420271 109.117206 translate
+0.000000 rotate
+0.000000 0.000000 m /hyphen glyphshow
+2.886719 0.000000 m /one glyphshow
+grestore
+0.500 setlinewidth
+2 setlinecap
+gsave
+416.3 257.7 12.9 12.09 clipbox
+12.904646 183.364461 m
+429.165354 183.364461 l
+stroke
+grestore
+0.800 setlinewidth
+0 setlinecap
+gsave
+/o {
+gsave
+newpath
+translate
+0.8 setlinewidth
+1 setlinejoin
+0 setlinecap
+0 0 m
+3.5 0 l
+
+gsave
+0.000 setgray
+fill
+grestore
+stroke
+grestore
+} bind def
+12.9046 183.364 o
+grestore
+gsave
+4.310896 180.325399 translate
+0.000000 rotate
+0.000000 0.000000 m /zero glyphshow
+grestore
+0.500 setlinewidth
+2 setlinecap
+gsave
+416.3 257.7 12.9 12.09 clipbox
+12.904646 254.572654 m
+429.165354 254.572654 l
+stroke
+grestore
+0.800 setlinewidth
+0 setlinecap
+gsave
+/o {
+gsave
+newpath
+translate
+0.8 setlinewidth
+1 setlinejoin
+0 setlinecap
+0 0 m
+3.5 0 l
+
+gsave
+0.000 setgray
+fill
+grestore
+stroke
+grestore
+} bind def
+12.9046 254.573 o
+grestore
+gsave
+4.310896 251.533591 translate
+0.000000 rotate
+0.000000 0.000000 m /one glyphshow
+grestore
+1.000 setlinewidth
+1 setlinecap
+0.000 0.606 0.979 setrgbcolor
+gsave
+416.3 257.7 12.9 12.09 clipbox
+24.685609 19.388345 m
+25.718458 35.859266 l
+26.876096 50.679253 l
+28.128322 63.847956 l
+29.431256 75.365421 l
+30.963311 86.876143 l
+32.489218 96.735178 l
+34.24241 106.585189 l
+35.901302 114.784578 l
+37.763687 122.973554 l
+39.854527 131.149402 l
+42.201845 139.308692 l
+44.28538 145.821334 l
+46.570988 152.317909 l
+49.078268 158.795135 l
+51.828719 165.249056 l
+54.845924 171.674899 l
+58.155756 178.066908 l
+60.847137 182.834584 l
+63.732005 187.575899 l
+66.82427 192.286876 l
+70.138842 196.96293 l
+73.691702 201.59877 l
+77.499983 206.188289 l
+81.582046 210.724431 l
+85.957574 215.199037 l
+90.647665 219.602666 l
+95.674932 223.924376 l
+101.063616 228.151473 l
+106.839699 232.269206 l
+110.919316 234.945236 l
+115.192189 237.559313 l
+119.66747 240.105032 l
+124.354751 242.57528 l
+129.264071 244.962148 l
+134.405951 247.256825 l
+139.791406 249.449484 l
+145.431975 251.529141 l
+151.339743 253.483496 l
+157.527367 255.298739 l
+164.008104 256.959329 l
+170.79584 258.44772 l
+177.905116 259.744039 l
+185.351166 260.825685 l
+193.149942 261.666846 l
+201.318152 262.237891 l
+209.873299 262.504604 l
+218.83371 262.427208 l
+228.218584 261.959094 l
+238.048027 261.045144 l
+243.136006 260.400887 l
+248.343101 259.619462 l
+253.672101 258.690435 l
+259.125861 257.602242 l
+264.707301 256.342007 l
+270.41941 254.895329 l
+276.265248 253.246008 l
+282.247945 251.375721 l
+288.370705 249.263604 l
+294.636808 246.885724 l
+301.049609 244.214408 l
+307.612543 241.217362 l
+314.329124 237.856506 l
+321.20295 234.08641 l
+328.237701 229.852141 l
+335.437146 225.08624 l
+342.80514 219.70436 l
+350.34563 213.598789 l
+358.062652 206.628469 l
+365.960341 198.602959 l
+374.042926 189.255314 l
+382.314736 178.19311 l
+390.7802 164.802377 l
+399.443852 148.036701 l
+408.310333 125.873203 l
+417.384391 93.496705 l
+417.384391 93.496705 l
+stroke
+grestore
+0 setlinejoin
+2 setlinecap
+[] 0 setdash
+0.000 setgray
+gsave
+12.904646 12.094646 m
+12.904646 269.805354 l
+stroke
+grestore
+gsave
+12.904646 12.094646 m
+429.165354 12.094646 l
+stroke
+grestore
+/DejaVuSans findfont
+14.000 scalefont
+setfont
+gsave
+205.917812 275.805354 translate
+0.000000 rotate
+0.000000 0.000000 m /p glyphshow
+8.886719 0.000000 m /l glyphshow
+12.776367 0.000000 m /o glyphshow
+21.341797 0.000000 m /p glyphshow
+grestore
+0 setlinecap
+gsave
+380.184104 251.655354 m
+423.565354 251.655354 l
+424.632021 251.655354 425.165354 252.188688 425.165354 253.255354 c
+425.165354 264.205354 l
+425.165354 265.272021 424.632021 265.805354 423.565354 265.805354 c
+380.184104 265.805354 l
+379.117438 265.805354 378.584104 265.272021 378.584104 264.205354 c
+378.584104 253.255354 l
+378.584104 252.188688 379.117438 251.655354 380.184104 251.655354 c
+cl
+gsave
+1.000 setgray
+fill
+grestore
+stroke
+grestore
+1 setlinejoin
+2 setlinecap
+[] 0 setdash
+0.000 0.606 0.979 setrgbcolor
+gsave
+381.784104 259.327229 m
+397.784104 259.327229 l
+stroke
+grestore
+0.000 setgray
+/DejaVuSans findfont
+8.000 scalefont
+setfont
+gsave
+404.184104 256.527229 translate
+0.000000 rotate
+0.000000 0.000000 m /m glyphshow
+7.792969 0.000000 m /a glyphshow
+12.695312 0.000000 m /g glyphshow
+grestore
+
+end
+showpage

utils/test_filter_bode.jl

@@ -0,0 +1,47 @@
+using Plots
+using DSP
+using CSV
+
+pyplot()
+
+# Parameters
+filename = joinpath(@__DIR__, "data", "walk.csv") # input file
+h = 1/22 # sample time
+orders = [
+    (title="quadratique", order=2, sizes=5:2:11),
+    (title="cubique", order=3, sizes=5:2:11),
+    (title="big_quadra", order=2, sizes=11:10:51)
+]
+
+
+function savgol(size::Int64, poly_order::Int64, deriv::Int64=0, delta::Float64=1.0, conv::Bool=false)
+    half_size, rem = divrem(size, 2)
+    if rem == 0
+        throw(ArgumentError("size must be odd."))
+    end
+    M = [-half_size:half_size;] .^ [0:poly_order;]';
+    y = zeros(poly_order+1)';
+    y[deriv+1] = factorial(deriv) / delta^deriv;
+    scal = y*inv(M'*M)*M'
+    if conv
+        scal = scal[end:-1:1]
+    end
+    scal
+end
+
+
+filter = savgol(19, 2, 1, h, true)
+H = tf(filter, [1], h)
+mag, phase, w = bode(H)
+plot(w,log.(mag[:,1]), label="mag", title="plop")
+savefig(joinpath(@__DIR__, "results", string("savgol_bode_", "plop", ".eps")))
+show()
+# for order in orders
+#     for size in order.sizes
+#         filter = savgol(size, order.order, 1, h, true)
+#         H = tf(filter, [1], 0.05)
+#         bode(H)
+#     end
+#     savefig(joinpath(@__DIR__, "results", string("savgol_bode_", order.title, ".eps")))
+# end
+# show()