diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6623bdf..526dcfc 100644
--- a/CMakeLists.txt
+++ b/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
)
###################################
diff --git a/cfg/RateNode.cfg b/cfg/RateNode.cfg
old mode 100644
new mode 100755
diff --git a/cfg/Triangle.cfg b/cfg/Triangle.cfg
new file mode 100755
index 0000000..22e0c8a
--- /dev/null
+++ b/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"))
diff --git a/launch/internal_tun.launch b/launch/internal_tun.launch
index 0a71f6a..7419f96 100644
--- a/launch/internal_tun.launch
+++ b/launch/internal_tun.launch
@@ -1,6 +1,12 @@
+
+
+
+
+
+
@@ -13,25 +19,22 @@
-
-
+
+
+
+
+
-
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
@@ -41,7 +44,11 @@
-
+
+
+
+
+
diff --git a/launch/test_derivative.launch b/launch/test_derivative.launch
index 69bc493..ba9975d 100644
--- a/launch/test_derivative.launch
+++ b/launch/test_derivative.launch
@@ -1,7 +1,14 @@
+
+
-
+
+
+
+
+
+
diff --git a/params/settings_blue.yaml b/params/settings_blue.yaml
new file mode 100644
index 0000000..c9335a4
--- /dev/null
+++ b/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: []
diff --git a/params/settings_safe.yaml b/params/settings_safe.yaml
new file mode 100644
index 0000000..afcfbcd
--- /dev/null
+++ b/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: []
diff --git a/params/settings_targets.yaml b/params/settings_targets.yaml
new file mode 100644
index 0000000..26fb602
--- /dev/null
+++ b/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: []
diff --git a/params/settings_test_D.yaml b/params/settings_test_D.yaml
new file mode 100644
index 0000000..74af80d
--- /dev/null
+++ b/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: []
diff --git a/params/settings_test_D_rate.yaml b/params/settings_test_D_rate.yaml
new file mode 100644
index 0000000..f7c767a
--- /dev/null
+++ b/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: []
diff --git a/params/settings_yellow.yaml b/params/settings_yellow.yaml
new file mode 100644
index 0000000..7444265
--- /dev/null
+++ b/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: []
diff --git a/scripts/control_compute.py b/scripts/control_compute.py
index 182774e..e5b4645 100755
--- a/scripts/control_compute.py
+++ b/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 SaturateNodeConfig, RateNodeConfig
+from detect_targets.cfg import DerivativeNodeConfig, InputNodeConfig
+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):
- self.rate = rospy.Rate(20)
- self.last_input = 0
+ def __init__(self, refresh_time=0.05):
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)
- self.rate = rospy.Rate(self.refresh_time)
+ c = super(RateNode, self).on_reconf(config, level)
+ 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()
diff --git a/scripts/publish_csv.py b/scripts/publish_csv.py
index e047e27..f40a701 100755
--- a/scripts/publish_csv.py
+++ b/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)
diff --git a/scripts/triangle.py b/scripts/triangle.py
index 19e11f8..25d5176 100755
--- a/scripts/triangle.py
+++ b/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()
diff --git a/utils/data/repind_vitesse.bag b/utils/data/repind_vitesse.bag
new file mode 100644
index 0000000..9df5057
Binary files /dev/null and b/utils/data/repind_vitesse.bag differ
diff --git a/utils/results/graph_test_d.png b/utils/results/graph_test_d.png
new file mode 100644
index 0000000..4dd1906
Binary files /dev/null and b/utils/results/graph_test_d.png differ
diff --git a/utils/results/mesure_vitesse_big_quadra.eps b/utils/results/mesure_vitesse_big_quadra.eps
index d90b7db..32437ae 100644
--- a/utils/results/mesure_vitesse_big_quadra.eps
+++ b/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
diff --git a/utils/results/mesure_vitesse_bode_big_quadra_11.eps b/utils/results/mesure_vitesse_bode_big_quadra_11.eps
new file mode 100644
index 0000000..e69de29
diff --git a/utils/results/mesure_vitesse_cubique.eps b/utils/results/mesure_vitesse_cubique.eps
index fdb5833..c0dbec8 100644
--- a/utils/results/mesure_vitesse_cubique.eps
+++ b/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
diff --git a/utils/results/mesure_vitesse_quadratique.eps b/utils/results/mesure_vitesse_quadratique.eps
index b0878f6..83ff25f 100644
--- a/utils/results/mesure_vitesse_quadratique.eps
+++ b/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
diff --git a/utils/results/savgol_bode_plop.eps b/utils/results/savgol_bode_plop.eps
new file mode 100644
index 0000000..183f602
--- /dev/null
+++ b/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
diff --git a/utils/results/savgol_bode_quadratique.eps b/utils/results/savgol_bode_quadratique.eps
new file mode 100644
index 0000000..e69de29
diff --git a/utils/test_filter_bode.jl b/utils/test_filter_bode.jl
new file mode 100644
index 0000000..302bd0b
--- /dev/null
+++ b/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()