Update figures and tangle matlab code

2020-11-24 18:28:16 +01:00 · 2020-11-24 18:28:16 +01:00 · 805af508fd
commit 805af508fd
parent 0b0c5d14a9
33 changed files with 5460 additions and 356 deletions
--- a/figs/apa95ml_5kg_PI_coh.pdf
+++ b/figs/apa95ml_5kg_PI_coh.pdf
--- a/figs/apa95ml_5kg_PI_coh.png
+++ b/figs/apa95ml_5kg_PI_coh.png
--- a/figs/apa95ml_5kg_cedrat_coh.pdf
+++ b/figs/apa95ml_5kg_cedrat_coh.pdf
--- a/figs/apa95ml_5kg_cedrat_coh.png
+++ b/figs/apa95ml_5kg_cedrat_coh.png
--- a/figs/apa95ml_5kg_pi_comp_fem.pdf
+++ b/figs/apa95ml_5kg_pi_comp_fem.pdf
--- a/figs/apa95ml_5kg_pi_comp_fem.png
+++ b/figs/apa95ml_5kg_pi_comp_fem.png
--- a/figs/bode_plot_force_sensor_voltage_comp_fem.pdf
+++ b/figs/bode_plot_force_sensor_voltage_comp_fem.pdf
--- a/figs/bode_plot_force_sensor_voltage_comp_fem.png
+++ b/figs/bode_plot_force_sensor_voltage_comp_fem.png
--- a/figs/dynamics_act_disp_comp_mass.pdf
+++ b/figs/dynamics_act_disp_comp_mass.pdf
--- a/figs/dynamics_act_disp_comp_mass.png
+++ b/figs/dynamics_act_disp_comp_mass.png
--- a/figs/dynamics_force_force_sensor_comp_mass.pdf
+++ b/figs/dynamics_force_force_sensor_comp_mass.pdf
--- a/figs/dynamics_force_force_sensor_comp_mass.png
+++ b/figs/dynamics_force_force_sensor_comp_mass.png
--- a/figs/iff_plant_identification_apa95ml.pdf
+++ b/figs/iff_plant_identification_apa95ml.pdf
--- a/figs/iff_plant_identification_apa95ml.png
+++ b/figs/iff_plant_identification_apa95ml.png
--- a/figs/test_bench_apa_identification.pdf
+++ b/figs/test_bench_apa_identification.pdf
--- a/figs/test_bench_apa_identification.png
+++ b/figs/test_bench_apa_identification.png
--- a/figs/test_bench_apa_identification.svg
+++ b/figs/test_bench_apa_identification.svg
--- a/figs/test_bench_apa_schematic.pdf
+++ b/figs/test_bench_apa_schematic.pdf
--- a/figs/test_bench_apa_schematic.png
+++ b/figs/test_bench_apa_schematic.png
--- a/figs/test_bench_apa_schematic.svg
+++ b/figs/test_bench_apa_schematic.svg
@ -15,7 +15,7 @@
   viewBox="0 0 445.25269 309.88137"
   sodipodi:docname="test_bench_apa_schematic.svg"
   inkscape:version="1.0.1 (3bc2e813f5, 2020-09-07)"
-   inkscape:export-filename="/home/thomas/Cloud/thesis/matlab/encoder-test-bench/figs/exp_setup_schematic.png"
+   inkscape:export-filename="/home/thomas/Cloud/thesis/matlab/test-bench-apa/figs/test_bench_apa_schematic.png"
   inkscape:export-xdpi="252"
   inkscape:export-ydpi="252">
  <metadata
@ -2594,15 +2594,85 @@
           id="path3184" />
      </symbol>
    </g>
    <g
       id="g5809">
      <symbol
         overflow="visible"
         id="symbol6882">
        <path
           style="stroke:none;"
           d=""
           id="path6251" />
      </symbol>
      <symbol
         overflow="visible"
         id="symbol9410">
        <path
           style="stroke:none;"
           d="M 6.265625 -5.671875 C 6.75 -6.453125 7.171875 -6.484375 7.53125 -6.5 C 7.65625 -6.515625 7.671875 -6.671875 7.671875 -6.6875 C 7.671875 -6.765625 7.609375 -6.8125 7.53125 -6.8125 C 7.28125 -6.8125 6.984375 -6.78125 6.71875 -6.78125 C 6.390625 -6.78125 6.046875 -6.8125 5.734375 -6.8125 C 5.671875 -6.8125 5.546875 -6.8125 5.546875 -6.625 C 5.546875 -6.515625 5.625 -6.5 5.703125 -6.5 C 5.96875 -6.484375 6.15625 -6.375 6.15625 -6.171875 C 6.15625 -6.015625 6.015625 -5.796875 6.015625 -5.796875 L 2.953125 -0.921875 L 2.265625 -6.203125 C 2.265625 -6.375 2.5 -6.5 2.953125 -6.5 C 3.09375 -6.5 3.203125 -6.5 3.203125 -6.703125 C 3.203125 -6.78125 3.125 -6.8125 3.078125 -6.8125 C 2.671875 -6.8125 2.25 -6.78125 1.828125 -6.78125 C 1.65625 -6.78125 1.46875 -6.78125 1.28125 -6.78125 C 1.109375 -6.78125 0.921875 -6.8125 0.75 -6.8125 C 0.671875 -6.8125 0.5625 -6.8125 0.5625 -6.625 C 0.5625 -6.5 0.640625 -6.5 0.8125 -6.5 C 1.359375 -6.5 1.375 -6.40625 1.40625 -6.15625 L 2.1875 -0.015625 C 2.21875 0.1875 2.265625 0.21875 2.390625 0.21875 C 2.546875 0.21875 2.59375 0.171875 2.671875 0.046875 Z M 6.265625 -5.671875 "
           id="path9059" />
      </symbol>
      <symbol
         overflow="visible"
         id="symbol8581">
        <path
           style="stroke:none;"
           d=""
           id="path5254" />
      </symbol>
      <symbol
         overflow="visible"
         id="symbol916">
        <path
           style="stroke:none;"
           d="M 2.9375 -2.65625 C 2.796875 -2.890625 2.5625 -3.078125 2.21875 -3.078125 C 1.328125 -3.078125 0.421875 -2.09375 0.421875 -1.09375 C 0.421875 -0.40625 0.875 0.0625 1.484375 0.0625 C 1.859375 0.0625 2.1875 -0.140625 2.46875 -0.421875 C 2.59375 0 3 0.0625 3.1875 0.0625 C 3.4375 0.0625 3.609375 -0.078125 3.734375 -0.296875 C 3.890625 -0.578125 3.984375 -0.96875 3.984375 -1 C 3.984375 -1.09375 3.890625 -1.09375 3.875 -1.09375 C 3.765625 -1.09375 3.765625 -1.0625 3.71875 -0.875 C 3.625 -0.53125 3.5 -0.125 3.203125 -0.125 C 3.03125 -0.125 2.96875 -0.28125 2.96875 -0.46875 C 2.96875 -0.578125 3.03125 -0.84375 3.078125 -1.015625 C 3.125 -1.203125 3.203125 -1.484375 3.234375 -1.640625 L 3.375 -2.171875 C 3.421875 -2.359375 3.5 -2.6875 3.5 -2.71875 C 3.5 -2.875 3.375 -2.9375 3.265625 -2.9375 C 3.140625 -2.9375 2.984375 -2.859375 2.9375 -2.65625 Z M 2.5 -0.875 C 2.453125 -0.671875 2.296875 -0.53125 2.140625 -0.40625 C 2.078125 -0.34375 1.796875 -0.125 1.5 -0.125 C 1.234375 -0.125 0.984375 -0.3125 0.984375 -0.796875 C 0.984375 -1.171875 1.1875 -1.9375 1.359375 -2.21875 C 1.671875 -2.78125 2.03125 -2.875 2.21875 -2.875 C 2.703125 -2.875 2.84375 -2.34375 2.84375 -2.265625 C 2.84375 -2.25 2.828125 -2.1875 2.828125 -2.171875 Z M 2.5 -0.875 "
           id="path7433" />
      </symbol>
    </g>
    <g
       id="g9659">
      <symbol
         overflow="visible"
         id="symbol2566">
        <path
           style="stroke:none;"
           d=""
           id="path2489" />
      </symbol>
      <symbol
         overflow="visible"
         id="symbol1277">
        <path
           style="stroke:none;"
           d="M 6.265625 -5.671875 C 6.75 -6.453125 7.171875 -6.484375 7.53125 -6.5 C 7.65625 -6.515625 7.671875 -6.671875 7.671875 -6.6875 C 7.671875 -6.765625 7.609375 -6.8125 7.53125 -6.8125 C 7.28125 -6.8125 6.984375 -6.78125 6.71875 -6.78125 C 6.390625 -6.78125 6.046875 -6.8125 5.734375 -6.8125 C 5.671875 -6.8125 5.546875 -6.8125 5.546875 -6.625 C 5.546875 -6.515625 5.625 -6.5 5.703125 -6.5 C 5.96875 -6.484375 6.15625 -6.375 6.15625 -6.171875 C 6.15625 -6.015625 6.015625 -5.796875 6.015625 -5.796875 L 2.953125 -0.921875 L 2.265625 -6.203125 C 2.265625 -6.375 2.5 -6.5 2.953125 -6.5 C 3.09375 -6.5 3.203125 -6.5 3.203125 -6.703125 C 3.203125 -6.78125 3.125 -6.8125 3.078125 -6.8125 C 2.671875 -6.8125 2.25 -6.78125 1.828125 -6.78125 C 1.65625 -6.78125 1.46875 -6.78125 1.28125 -6.78125 C 1.109375 -6.78125 0.921875 -6.8125 0.75 -6.8125 C 0.671875 -6.8125 0.5625 -6.8125 0.5625 -6.625 C 0.5625 -6.5 0.640625 -6.5 0.8125 -6.5 C 1.359375 -6.5 1.375 -6.40625 1.40625 -6.15625 L 2.1875 -0.015625 C 2.21875 0.1875 2.265625 0.21875 2.390625 0.21875 C 2.546875 0.21875 2.59375 0.171875 2.671875 0.046875 Z M 6.265625 -5.671875 "
           id="path6334" />
      </symbol>
      <symbol
         overflow="visible"
         id="symbol2610">
        <path
           style="stroke:none;"
           d=""
           id="path4350" />
      </symbol>
      <symbol
         overflow="visible"
         id="symbol3419">
        <path
           style="stroke:none;"
           d="M 3 -2.625 C 2.828125 -2.578125 2.703125 -2.4375 2.703125 -2.296875 C 2.703125 -2.125 2.84375 -2.0625 2.9375 -2.0625 C 3.015625 -2.0625 3.28125 -2.109375 3.28125 -2.46875 C 3.28125 -2.921875 2.78125 -3.078125 2.34375 -3.078125 C 1.265625 -3.078125 1.0625 -2.265625 1.0625 -2.0625 C 1.0625 -1.796875 1.21875 -1.625 1.3125 -1.546875 C 1.484375 -1.40625 1.609375 -1.375 2.09375 -1.296875 C 2.25 -1.265625 2.6875 -1.1875 2.6875 -0.84375 C 2.6875 -0.71875 2.609375 -0.453125 2.328125 -0.28125 C 2.046875 -0.125 1.703125 -0.125 1.625 -0.125 C 1.34375 -0.125 0.953125 -0.1875 0.78125 -0.421875 C 1.015625 -0.453125 1.171875 -0.625 1.171875 -0.8125 C 1.171875 -0.984375 1.046875 -1.078125 0.90625 -1.078125 C 0.703125 -1.078125 0.5 -0.90625 0.5 -0.609375 C 0.5 -0.1875 0.9375 0.0625 1.609375 0.0625 C 2.90625 0.0625 3.140625 -0.8125 3.140625 -1.078125 C 3.140625 -1.71875 2.4375 -1.84375 2.171875 -1.890625 C 2.109375 -1.90625 1.9375 -1.9375 1.890625 -1.953125 C 1.640625 -2 1.515625 -2.140625 1.515625 -2.296875 C 1.515625 -2.453125 1.640625 -2.640625 1.796875 -2.75 C 1.984375 -2.859375 2.21875 -2.875 2.34375 -2.875 C 2.484375 -2.875 2.84375 -2.859375 3 -2.625 Z M 3 -2.625 "
           id="path8818" />
      </symbol>
    </g>
  </defs>
  <sodipodi:namedview
     inkscape:current-layer="layer4"
     inkscape:window-maximized="1"
     inkscape:window-y="39"
     inkscape:window-x="12"
-     inkscape:cy="160.51458"
+     inkscape:cy="156.38525"
-     inkscape:cx="184.38069"
+     inkscape:cx="236.24483"
-     inkscape:zoom="2.0000001"
+     inkscape:zoom="2.8284273"
     showgrid="false"
     id="namedview835"
     inkscape:window-height="1389"
@ -4051,5 +4121,65 @@
        </g>
      </g>
    </g>
    <g
       inkscape:label=""
       transform="translate(-321.07901,188.62035)"
       id="g2857">
      <g
         id="g2157">
        <g
           style="fill:#000000;fill-opacity:1"
           id="g8295">
          <use
             xlink:href="#symbol9410"
             x="300.68399"
             y="93.917999"
             id="use7298"
             width="100%"
             height="100%" />
        </g>
        <g
           style="fill:#000000;fill-opacity:1"
           id="g8892">
          <use
             xlink:href="#symbol916"
             x="306.496"
             y="95.412003"
             id="use2362"
             width="100%"
             height="100%" />
        </g>
      </g>
    </g>
    <g
       inkscape:label=""
       transform="translate(-316.13934,138.59343)"
       id="g3721">
      <g
         id="g5893">
        <g
           style="fill:#000000;fill-opacity:1"
           id="g6514">
          <use
             xlink:href="#symbol1277"
             x="300.965"
             y="93.917999"
             id="use3541"
             width="100%"
             height="100%" />
        </g>
        <g
           style="fill:#000000;fill-opacity:1"
           id="g3422">
          <use
             xlink:href="#symbol3419"
             x="306.77701"
             y="95.412003"
             id="use8375"
             width="100%"
             height="100%" />
        </g>
      </g>
    </g>
  </g>
 </svg>
--- a/figs/test_bench_apa_schematic_iff.pdf
+++ b/figs/test_bench_apa_schematic_iff.pdf
--- a/figs/test_bench_apa_schematic_iff.png
+++ b/figs/test_bench_apa_schematic_iff.png
--- a/figs/test_bench_apa_schematic_iff.svg
+++ b/figs/test_bench_apa_schematic_iff.svg
@ -26,7 +26,7 @@
        <dc:format>image/svg+xml</dc:format>
        <dc:type
           rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
-        <dc:title></dc:title>
+        <dc:title />
      </cc:Work>
    </rdf:RDF>
  </metadata>
@ -2706,18 +2706,88 @@
           id="path5148" />
      </symbol>
    </g>
    <g
       id="g7876">
      <symbol
         overflow="visible"
         id="symbol6801">
        <path
           style="stroke:none;"
           d=""
           id="path9069" />
      </symbol>
      <symbol
         overflow="visible"
         id="symbol1726">
        <path
           style="stroke:none;"
           d="M 6.265625 -5.671875 C 6.75 -6.453125 7.171875 -6.484375 7.53125 -6.5 C 7.65625 -6.515625 7.671875 -6.671875 7.671875 -6.6875 C 7.671875 -6.765625 7.609375 -6.8125 7.53125 -6.8125 C 7.28125 -6.8125 6.984375 -6.78125 6.71875 -6.78125 C 6.390625 -6.78125 6.046875 -6.8125 5.734375 -6.8125 C 5.671875 -6.8125 5.546875 -6.8125 5.546875 -6.625 C 5.546875 -6.515625 5.625 -6.5 5.703125 -6.5 C 5.96875 -6.484375 6.15625 -6.375 6.15625 -6.171875 C 6.15625 -6.015625 6.015625 -5.796875 6.015625 -5.796875 L 2.953125 -0.921875 L 2.265625 -6.203125 C 2.265625 -6.375 2.5 -6.5 2.953125 -6.5 C 3.09375 -6.5 3.203125 -6.5 3.203125 -6.703125 C 3.203125 -6.78125 3.125 -6.8125 3.078125 -6.8125 C 2.671875 -6.8125 2.25 -6.78125 1.828125 -6.78125 C 1.65625 -6.78125 1.46875 -6.78125 1.28125 -6.78125 C 1.109375 -6.78125 0.921875 -6.8125 0.75 -6.8125 C 0.671875 -6.8125 0.5625 -6.8125 0.5625 -6.625 C 0.5625 -6.5 0.640625 -6.5 0.8125 -6.5 C 1.359375 -6.5 1.375 -6.40625 1.40625 -6.15625 L 2.1875 -0.015625 C 2.21875 0.1875 2.265625 0.21875 2.390625 0.21875 C 2.546875 0.21875 2.59375 0.171875 2.671875 0.046875 Z M 6.265625 -5.671875 "
           id="path9494" />
      </symbol>
      <symbol
         overflow="visible"
         id="symbol8930">
        <path
           style="stroke:none;"
           d=""
           id="path2341" />
      </symbol>
      <symbol
         overflow="visible"
         id="symbol3791">
        <path
           style="stroke:none;"
           d="M 3 -2.625 C 2.828125 -2.578125 2.703125 -2.4375 2.703125 -2.296875 C 2.703125 -2.125 2.84375 -2.0625 2.9375 -2.0625 C 3.015625 -2.0625 3.28125 -2.109375 3.28125 -2.46875 C 3.28125 -2.921875 2.78125 -3.078125 2.34375 -3.078125 C 1.265625 -3.078125 1.0625 -2.265625 1.0625 -2.0625 C 1.0625 -1.796875 1.21875 -1.625 1.3125 -1.546875 C 1.484375 -1.40625 1.609375 -1.375 2.09375 -1.296875 C 2.25 -1.265625 2.6875 -1.1875 2.6875 -0.84375 C 2.6875 -0.71875 2.609375 -0.453125 2.328125 -0.28125 C 2.046875 -0.125 1.703125 -0.125 1.625 -0.125 C 1.34375 -0.125 0.953125 -0.1875 0.78125 -0.421875 C 1.015625 -0.453125 1.171875 -0.625 1.171875 -0.8125 C 1.171875 -0.984375 1.046875 -1.078125 0.90625 -1.078125 C 0.703125 -1.078125 0.5 -0.90625 0.5 -0.609375 C 0.5 -0.1875 0.9375 0.0625 1.609375 0.0625 C 2.90625 0.0625 3.140625 -0.8125 3.140625 -1.078125 C 3.140625 -1.71875 2.4375 -1.84375 2.171875 -1.890625 C 2.109375 -1.90625 1.9375 -1.9375 1.890625 -1.953125 C 1.640625 -2 1.515625 -2.140625 1.515625 -2.296875 C 1.515625 -2.453125 1.640625 -2.640625 1.796875 -2.75 C 1.984375 -2.859375 2.21875 -2.875 2.34375 -2.875 C 2.484375 -2.875 2.84375 -2.859375 3 -2.625 Z M 3 -2.625 "
           id="path5227" />
      </symbol>
    </g>
    <g
       id="g7988">
      <symbol
         overflow="visible"
         id="symbol336">
        <path
           style="stroke:none;"
           d=""
           id="path5757" />
      </symbol>
      <symbol
         overflow="visible"
         id="symbol1532">
        <path
           style="stroke:none;"
           d="M 6.265625 -5.671875 C 6.75 -6.453125 7.171875 -6.484375 7.53125 -6.5 C 7.65625 -6.515625 7.671875 -6.671875 7.671875 -6.6875 C 7.671875 -6.765625 7.609375 -6.8125 7.53125 -6.8125 C 7.28125 -6.8125 6.984375 -6.78125 6.71875 -6.78125 C 6.390625 -6.78125 6.046875 -6.8125 5.734375 -6.8125 C 5.671875 -6.8125 5.546875 -6.8125 5.546875 -6.625 C 5.546875 -6.515625 5.625 -6.5 5.703125 -6.5 C 5.96875 -6.484375 6.15625 -6.375 6.15625 -6.171875 C 6.15625 -6.015625 6.015625 -5.796875 6.015625 -5.796875 L 2.953125 -0.921875 L 2.265625 -6.203125 C 2.265625 -6.375 2.5 -6.5 2.953125 -6.5 C 3.09375 -6.5 3.203125 -6.5 3.203125 -6.703125 C 3.203125 -6.78125 3.125 -6.8125 3.078125 -6.8125 C 2.671875 -6.8125 2.25 -6.78125 1.828125 -6.78125 C 1.65625 -6.78125 1.46875 -6.78125 1.28125 -6.78125 C 1.109375 -6.78125 0.921875 -6.8125 0.75 -6.8125 C 0.671875 -6.8125 0.5625 -6.8125 0.5625 -6.625 C 0.5625 -6.5 0.640625 -6.5 0.8125 -6.5 C 1.359375 -6.5 1.375 -6.40625 1.40625 -6.15625 L 2.1875 -0.015625 C 2.21875 0.1875 2.265625 0.21875 2.390625 0.21875 C 2.546875 0.21875 2.59375 0.171875 2.671875 0.046875 Z M 6.265625 -5.671875 "
           id="path4616" />
      </symbol>
      <symbol
         overflow="visible"
         id="symbol3915">
        <path
           style="stroke:none;"
           d=""
           id="path447" />
      </symbol>
      <symbol
         overflow="visible"
         id="symbol9907">
        <path
           style="stroke:none;"
           d="M 2.9375 -2.65625 C 2.796875 -2.890625 2.5625 -3.078125 2.21875 -3.078125 C 1.328125 -3.078125 0.421875 -2.09375 0.421875 -1.09375 C 0.421875 -0.40625 0.875 0.0625 1.484375 0.0625 C 1.859375 0.0625 2.1875 -0.140625 2.46875 -0.421875 C 2.59375 0 3 0.0625 3.1875 0.0625 C 3.4375 0.0625 3.609375 -0.078125 3.734375 -0.296875 C 3.890625 -0.578125 3.984375 -0.96875 3.984375 -1 C 3.984375 -1.09375 3.890625 -1.09375 3.875 -1.09375 C 3.765625 -1.09375 3.765625 -1.0625 3.71875 -0.875 C 3.625 -0.53125 3.5 -0.125 3.203125 -0.125 C 3.03125 -0.125 2.96875 -0.28125 2.96875 -0.46875 C 2.96875 -0.578125 3.03125 -0.84375 3.078125 -1.015625 C 3.125 -1.203125 3.203125 -1.484375 3.234375 -1.640625 L 3.375 -2.171875 C 3.421875 -2.359375 3.5 -2.6875 3.5 -2.71875 C 3.5 -2.875 3.375 -2.9375 3.265625 -2.9375 C 3.140625 -2.9375 2.984375 -2.859375 2.9375 -2.65625 Z M 2.5 -0.875 C 2.453125 -0.671875 2.296875 -0.53125 2.140625 -0.40625 C 2.078125 -0.34375 1.796875 -0.125 1.5 -0.125 C 1.234375 -0.125 0.984375 -0.3125 0.984375 -0.796875 C 0.984375 -1.171875 1.1875 -1.9375 1.359375 -2.21875 C 1.671875 -2.78125 2.03125 -2.875 2.21875 -2.875 C 2.703125 -2.875 2.84375 -2.34375 2.84375 -2.265625 C 2.84375 -2.25 2.828125 -2.1875 2.828125 -2.171875 Z M 2.5 -0.875 "
           id="path6699" />
      </symbol>
    </g>
  </defs>
  <sodipodi:namedview
     inkscape:current-layer="g11471"
     inkscape:window-maximized="1"
-     inkscape:window-y="57"
+     inkscape:window-y="39"
     inkscape:window-x="12"
-     inkscape:cy="153.10225"
+     inkscape:cy="242.40146"
-     inkscape:cx="191.54895"
+     inkscape:cx="156.32842"
-     inkscape:zoom="2.8284272"
+     inkscape:zoom="11.313709"
     showgrid="false"
     id="namedview835"
-     inkscape:window-height="1371"
+     inkscape:window-height="1389"
     inkscape:window-width="2536"
     inkscape:pageshadow="2"
     inkscape:pageopacity="0"
@ -3201,6 +3271,66 @@
            </g>
          </g>
        </g>
        <g
           inkscape:label=""
           transform="translate(-213.41053,145.41162)"
           id="g2777">
          <g
             id="g6873">
            <g
               style="fill:#000000;fill-opacity:1"
               id="g6118">
              <use
                 xlink:href="#symbol1726"
                 x="300.965"
                 y="93.917999"
                 id="use6749"
                 width="100%"
                 height="100%" />
            </g>
            <g
               style="fill:#000000;fill-opacity:1"
               id="g4781">
              <use
                 xlink:href="#symbol3791"
                 x="306.77701"
                 y="95.412003"
                 id="use1702"
                 width="100%"
                 height="100%" />
            </g>
          </g>
        </g>
        <g
           inkscape:label=""
           transform="translate(-218.84278,194.41367)"
           id="g3692">
          <g
             id="g4069">
            <g
               style="fill:#000000;fill-opacity:1"
               id="g4739">
              <use
                 xlink:href="#symbol1532"
                 x="300.68399"
                 y="93.917999"
                 id="use5441"
                 width="100%"
                 height="100%" />
            </g>
            <g
               style="fill:#000000;fill-opacity:1"
               id="g2990">
              <use
                 xlink:href="#symbol9907"
                 x="306.496"
                 y="95.412003"
                 id="use188"
                 width="100%"
                 height="100%" />
            </g>
          </g>
        </g>
      </g>
    </g>
  </g>
--- a/index.html
+++ b/index.html
@ -3,7 +3,7 @@
 "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
 <html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
 <head>
-<!-- 2020-11-24 mar. 13:54 -->
+<!-- 2020-11-24 mar. 18:24 -->
 <meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
 <title>Test Bench - Amplified Piezoelectric Actuator</title>
 <meta name="generator" content="Org mode" />
@ -30,75 +30,76 @@
 <h2>Table of Contents</h2>
 <div id="text-table-of-contents">
 <ul>
-<li><a href="#org692da4d">1. Experimental Setup</a></li>
+<li><a href="#orgb908526">1. Experimental Setup</a></li>
-<li><a href="#orgf5c6cd7">2. Estimation of electrical/mechanical relationships</a>
+<li><a href="#org58c6b68">2. Estimation of electrical/mechanical relationships</a>
 <ul>
-<li><a href="#orgd8c7c0b">2.1. Estimation from Data-sheet</a></li>
+<li><a href="#org5a0dde5">2.1. Estimation from Data-sheet</a></li>
-<li><a href="#org901da31">2.2. Estimation from Piezoelectric parameters</a></li>
+<li><a href="#org20f0327">2.2. Estimation from Piezoelectric parameters</a></li>
-<li><a href="#org616104b">2.3. Estimation from Experiment</a>
+<li><a href="#org29014d6">2.3. Estimation from Experiment</a>
 <ul>
-<li><a href="#orgcb3d952">2.3.1. From actuator voltage \(V_a\) to actuator force \(F_a\)</a></li>
+<li><a href="#org3f63fed">2.3.1. From actuator voltage \(V_a\) to actuator force \(F_a\)</a></li>
-<li><a href="#org138110f">2.3.2. From stack strain \(\Delta h\) to generated voltage \(V_s\)</a></li>
+<li><a href="#org0877a6d">2.3.2. From stack strain \(\Delta h\) to generated voltage \(V_s\)</a></li>
 </ul>
 </li>
-<li><a href="#org7cf58ef">2.4. Conclusion</a></li>
+<li><a href="#org5e8b78e">2.4. Conclusion</a></li>
 </ul>
 </li>
-<li><a href="#org36bf85c">3. Simscape model of the test-bench</a>
+<li><a href="#orgaded65a">3. Simscape model of the test-bench</a>
 <ul>
-<li><a href="#orgca28e7b">3.1. Import Mass Matrix, Stiffness Matrix, and Interface Nodes Coordinates</a></li>
+<li><a href="#orgbd18d02">3.1. Import Mass Matrix, Stiffness Matrix, and Interface Nodes Coordinates</a></li>
-<li><a href="#orga49ef8e">3.2. Simscape Model</a></li>
+<li><a href="#org6df0376">3.2. Simscape Model</a></li>
-<li><a href="#orgf1f3f75">3.3. Dynamics from Actuator Voltage to Vertical Mass Displacement</a></li>
+<li><a href="#org1d92e82">3.3. Dynamics from Actuator Voltage to Vertical Mass Displacement</a></li>
-<li><a href="#orgd4a750c">3.4. Dynamics from Actuator Voltage to Force Sensor Voltage</a></li>
+<li><a href="#org18d1e8d">3.4. Dynamics from Actuator Voltage to Force Sensor Voltage</a></li>
-<li><a href="#org1ac4d3c">3.5. Save Data for further use</a></li>
+<li><a href="#org74335e3">3.5. Save Data for further use</a></li>
 </ul>
 </li>
-<li><a href="#org9de639d">4. Huddle Test</a>
+<li><a href="#org98bd921">4. Measurement of the ambient noise in the system</a>
 <ul>
-<li><a href="#orgaf93c0c">4.1. Time Domain Data</a></li>
+<li><a href="#orge831f73">4.1. Time Domain Data</a></li>
-<li><a href="#org22ccdcf">4.2. PSD of Measurement Noise</a></li>
+<li><a href="#org38d16b4">4.2. PSD of Measurement Noise</a></li>
 </ul>
 </li>
-<li><a href="#org914a7da">5. Identification of the dynamics from actuator to displacement</a>
+<li><a href="#org90553e0">5. Identification of the dynamics from actuator Voltage to displacement</a>
 <ul>
-<li><a href="#orga1aace1">5.1. Load Data</a></li>
+<li><a href="#orgbdf7a7b">5.1. Load Data</a></li>
-<li><a href="#org1284757">5.2. Comparison of the PSD with Huddle Test</a></li>
+<li><a href="#org5513479">5.2. Comparison of the PSD with Huddle Test</a></li>
-<li><a href="#org89453e3">5.3. Compute TF estimate and Coherence</a></li>
+<li><a href="#org4d4bebc">5.3. Compute TF estimate and Coherence</a></li>
 </ul>
 </li>
-<li><a href="#orgfff18d2">6. Identification of the dynamics from actuator to force sensor</a>
+<li><a href="#org12553c5">6. Identification of the dynamics from actuator Voltage to force sensor Voltage</a></li>
 <li><a href="#org6f69286">7. Integral Force Feedback</a>
 <ul>
-<li><a href="#orgfa2171a">6.1. System Identification</a></li>
+<li><a href="#orgbc62cba">7.1. IFF Plant</a></li>
-<li><a href="#org633e9f1">6.2. Integral Force Feedback</a></li>
+<li><a href="#org4d62e16">7.2. First tests with few gains</a></li>
-</ul>
+<li><a href="#org4304a4e">7.3. Second test with many Gains</a></li>
 </li>
 <li><a href="#org1bf58e2">7. Integral Force Feedback</a>
 <ul>
 <li><a href="#org25092d8">7.1. First tests with few gains</a></li>
 <li><a href="#org67fa466">7.2. Second test with many Gains</a></li>
 </ul>
 </li>
 </ul>
 </div>
 </div>
 <p>
 This document is divided in the following sections:
 </p>
 <ul class="org-ul">
-<li>Section <a href="#org2c6bb95">1</a>:</li>
+<li>Section <a href="#orgac1a70e">1</a>: the experimental setup is described</li>
-<li>Section <a href="#orgb81d61b">3</a>:</li>
+<li>Section <a href="#org0db4f21">2</a>: the parameters which are important for the Simscape model of the piezoelectric stack actuator and sensors are estimated</li>
-<li>Section [[]]:</li>
+<li>Section <a href="#org9d27452">3</a>: the Simscape model of the test bench is presented</li>
-<li>Section [[]]:</li>
+<li>Section <a href="#orga049d7f">4</a>: as usual, a first measurement of the noise/disturbances present in the system is performed</li>
-<li>Section [[]]:</li>
+<li>Section <a href="#org72688d3">5</a>: the transfer function from the actuator voltage to the displacement of the mass is identified and compared with the model</li>
 <li>Section <a href="#orgdd284b9">6</a>: the tranfer function from the actuator voltage to the sensor stack voltage is identified and compare with the model</li>
 <li>Section <a href="#org13dae17">7</a>: the Integral Force Feedback control architecture is applied on the system using the force sensor stack in order to add damping to the suspension resonance</li>
 </ul>
-<div id="outline-container-org692da4d" class="outline-2">
+<div id="outline-container-orgb908526" class="outline-2">
-<h2 id="org692da4d"><span class="section-number-2">1</span> Experimental Setup</h2>
+<h2 id="orgb908526"><span class="section-number-2">1</span> Experimental Setup</h2>
 <div class="outline-text-2" id="text-1">
 <p>
-<a id="org2c6bb95"></a>
+<a id="orgac1a70e"></a>
 </p>
 <p>
-A schematic of the test-bench is shown in Figure <a href="#org57478f1">1</a>.
+A schematic of the test-bench is shown in Figure <a href="#org42f14fa">1</a>.
 </p>
 <p>
@ -111,31 +112,31 @@ The APA95ML has three stacks that can be used as actuator or as sensors.
 </p>
 <p>
-Pictures of the test bench are shown in Figure <a href="#org4bf2105">2</a> and <a href="#org809b25b">3</a>.
+Pictures of the test bench are shown in Figure <a href="#org934d0a3">2</a> and <a href="#orgf6513b0">3</a>.
 </p>
-<div id="org57478f1" class="figure">
+<div id="org42f14fa" class="figure">
 <p><img src="figs/test_bench_apa_schematic.png" alt="test_bench_apa_schematic.png" />
 </p>
 <p><span class="figure-number">Figure 1: </span>Schematic of the Setup</p>
 </div>
-<div id="org4bf2105" class="figure">
+<div id="org934d0a3" class="figure">
 <p><img src="figs/setup_picture.png" alt="setup_picture.png" />
 </p>
 <p><span class="figure-number">Figure 2: </span>Picture of the Setup</p>
 </div>
-<div id="org809b25b" class="figure">
+<div id="orgf6513b0" class="figure">
 <p><img src="figs/setup_zoom.png" alt="setup_zoom.png" />
 </p>
 <p><span class="figure-number">Figure 3: </span>Zoom on the APA</p>
 </div>
-<div class="note" id="org79ca795">
+<div class="note" id="orgb97925c">
 <p>
 Here are the equipment used in the test bench:
 </p>
@ -151,11 +152,14 @@ Here are the equipment used in the test bench:
 </div>
 </div>
-<div id="outline-container-orgf5c6cd7" class="outline-2">
+<div id="outline-container-org58c6b68" class="outline-2">
-<h2 id="orgf5c6cd7"><span class="section-number-2">2</span> Estimation of electrical/mechanical relationships</h2>
+<h2 id="org58c6b68"><span class="section-number-2">2</span> Estimation of electrical/mechanical relationships</h2>
 <div class="outline-text-2" id="text-2">
 <p>
-In order to correctly model the piezoelectric actuator, we need to determine:
+<a id="org0db4f21"></a>
 </p>
 <p>
 In order to correctly model the piezoelectric actuator with Simscape, we need to determine:
 </p>
 <ol class="org-ol">
 <li>\(g_a\): the ratio of the generated force \(F_a\) to the supply voltage \(V_a\) across the piezoelectric stack</li>
@ -166,24 +170,23 @@ In order to correctly model the piezoelectric actuator, we need to determine:
 We estimate \(g_a\) and \(g_s\) using different approaches:
 </p>
 <ol class="org-ol">
-<li>Section <a href="#org640a15c">2.1</a>: \(g_a\) is estimated from the datasheet of the piezoelectric stack</li>
+<li>Section <a href="#orgcfc54fa">2.1</a>: \(g_a\) is estimated from the datasheet of the piezoelectric stack</li>
-<li>Section <a href="#orgeeaae90">2.2</a>: \(g_a\) and \(g_s\) are estimated using the piezoelectric constants</li>
+<li>Section <a href="#org0db4f21">2</a>: \(g_a\) and \(g_s\) are estimated using the piezoelectric constants</li>
-<li>Section <a href="#org5a92750">2.3</a>: \(g_a\) and \(g_s\) are estimated experimentally</li>
+<li>Section <a href="#orgcdcd11f">2.3</a>: \(g_a\) and \(g_s\) are estimated experimentally</li>
 </ol>
 </div>
-
+<div id="outline-container-org5a0dde5" class="outline-3">
-<div id="outline-container-orgd8c7c0b" class="outline-3">
+<h3 id="org5a0dde5"><span class="section-number-3">2.1</span> Estimation from Data-sheet</h3>
 <h3 id="orgd8c7c0b"><span class="section-number-3">2.1</span> Estimation from Data-sheet</h3>
 <div class="outline-text-3" id="text-2-1">
 <p>
-<a id="org640a15c"></a>
+<a id="orgcfc54fa"></a>
 </p>
 <p>
-The stack parameters taken from the data-sheet are shown in Table <a href="#orgf2c9009">1</a>.
+The stack parameters taken from the data-sheet are shown in Table <a href="#org82c2f0a">1</a>.
 </p>
-<table id="orgf2c9009" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
+<table id="org82c2f0a" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
 <caption class="t-above"><span class="table-number">Table 1:</span> Stack Parameters</caption>
 <colgroup>
@ -269,12 +272,12 @@ Vmax = 170; <span class="org-comment">% [V]</span>
 </div>
 <div class="org-src-container">
-<pre class="src src-matlab">ka<span class="org-type">*</span>Lmax<span class="org-type">/</span>Vmax <span class="org-comment">% [N/V]</span>
+<pre class="src src-matlab">ga = ka<span class="org-type">*</span>Lmax<span class="org-type">/</span>Vmax; <span class="org-comment">% [N/V]</span>
 </pre>
 </div>
 <pre class="example">
-27.647
+ga = 27.6 [N/V]
 </pre>
@ -284,11 +287,11 @@ From the parameters of the stack, it seems not possible to estimate the relation
 </div>
 </div>
-<div id="outline-container-org901da31" class="outline-3">
+<div id="outline-container-org20f0327" class="outline-3">
-<h3 id="org901da31"><span class="section-number-3">2.2</span> Estimation from Piezoelectric parameters</h3>
+<h3 id="org20f0327"><span class="section-number-3">2.2</span> Estimation from Piezoelectric parameters</h3>
 <div class="outline-text-3" id="text-2-2">
 <p>
-<a id="orgeeaae90"></a>
+<a id="orgc2edf63"></a>
 </p>
 <p>
@ -305,7 +308,7 @@ ka  = 235e6;   <span class="org-comment">% Stack stiffness [N/m]</span>
 The ratio of the developed force to applied voltage is:
 </p>
 \begin{equation}
-\label{org8300de8}
+\label{org6f1476c}
  F_a = g_a V_a, \quad g_a = d_{33} n k_a
 \end{equation}
 <p>
@ -337,7 +340,7 @@ ga = 5.6 [N/V]
 From (<a href="#citeproc_bib_item_1">Fleming and Leang 2014</a>) (page 123), the relation between relative displacement of the sensor stack and generated voltage is:
 </p>
 \begin{equation}
-\label{org312083f}
+\label{org43f15ff}
  V_s = \frac{d_{33}}{\epsilon^T s^D n} \Delta h
 \end{equation}
 <p>
@ -374,11 +377,11 @@ gs = 35.4 [V/um]
 </div>
 </div>
-<div id="outline-container-org616104b" class="outline-3">
+<div id="outline-container-org29014d6" class="outline-3">
-<h3 id="org616104b"><span class="section-number-3">2.3</span> Estimation from Experiment</h3>
+<h3 id="org29014d6"><span class="section-number-3">2.3</span> Estimation from Experiment</h3>
 <div class="outline-text-3" id="text-2-3">
 <p>
-<a id="org5a92750"></a>
+<a id="orgcdcd11f"></a>
 </p>
 <p>
 The idea here is to obtain the parameters \(g_a\) and \(g_s\) from the comparison of an experimental identification and the identification using Simscape.
@ -394,8 +397,8 @@ Similarly, it is fairly easy to experimentally obtain the gain from the stack di
 To link that to the strain of the sensor stack, the simscape model is used.
 </p>
 </div>
-<div id="outline-container-orgcb3d952" class="outline-4">
+<div id="outline-container-org3f63fed" class="outline-4">
-<h4 id="orgcb3d952"><span class="section-number-4">2.3.1</span> From actuator voltage \(V_a\) to actuator force \(F_a\)</h4>
+<h4 id="org3f63fed"><span class="section-number-4">2.3.1</span> From actuator voltage \(V_a\) to actuator force \(F_a\)</h4>
 <div class="outline-text-4" id="text-2-3-1">
 <p>
 The data from the identification test is loaded.
@ -439,7 +442,7 @@ The gain from input voltage of the stack to the vertical displacement is determi
 </div>
-<div id="orgf3201ac" class="figure">
+<div id="org85d884f" class="figure">
 <p><img src="figs/gain_Va_to_d.png" alt="gain_Va_to_d.png" />
 </p>
 <p><span class="figure-number">Figure 4: </span>Transfer function from actuator stack voltage \(V_a\) to vertical displacement of the mass \(d\)</p>
@ -495,11 +498,11 @@ ga = 33.7 [N/V]
 <p>
-The obtained comparison between the Simscape model and the identified dynamics is shown in Figure <a href="#orgc727518">5</a>.
+The obtained comparison between the Simscape model and the identified dynamics is shown in Figure <a href="#orgc82fdbe">5</a>.
 </p>
-<div id="orgc727518" class="figure">
+<div id="orgc82fdbe" class="figure">
 <p><img src="figs/compare_Gd_id_simscape.png" alt="compare_Gd_id_simscape.png" />
 </p>
 <p><span class="figure-number">Figure 5: </span>Comparison of the identified transfer function between actuator voltage \(V_a\) and vertical mass displacement \(d\)</p>
@ -507,8 +510,8 @@ The obtained comparison between the Simscape model and the identified dynamics i
 </div>
 </div>
-<div id="outline-container-org138110f" class="outline-4">
+<div id="outline-container-org0877a6d" class="outline-4">
-<h4 id="org138110f"><span class="section-number-4">2.3.2</span> From stack strain \(\Delta h\) to generated voltage \(V_s\)</h4>
+<h4 id="org0877a6d"><span class="section-number-4">2.3.2</span> From stack strain \(\Delta h\) to generated voltage \(V_s\)</h4>
 <div class="outline-text-4" id="text-2-3-2">
 <p>
 Now, the gain from the stack strain \(\Delta h\) to the generated voltage \(V_s\) is estimated.
@ -543,7 +546,7 @@ Here, an amplifier with a gain of 20 is used.
 </div>
 <p>
-Then, the transfer function from \(V_a\) to \(V_s\) is identified and its DC gain is estimated (Figure <a href="#org6245a69">6</a>).
+Then, the transfer function from \(V_a\) to \(V_s\) is identified and its DC gain is estimated (Figure <a href="#orgf3891ce">6</a>).
 </p>
 <div class="org-src-container">
 <pre class="src src-matlab">Ts = t(end)<span class="org-type">/</span>(length(t)<span class="org-type">-</span>1);
@ -561,7 +564,7 @@ win = hann(ceil(10<span class="org-type">/</span>Ts));
 </div>
-<div id="org6245a69" class="figure">
+<div id="orgf3891ce" class="figure">
 <p><img src="figs/gain_Va_to_Vs.png" alt="gain_Va_to_Vs.png" />
 </p>
 <p><span class="figure-number">Figure 6: </span>Transfer function from actuator stack voltage \(V_a\) to sensor stack voltage \(V_s\)</p>
@ -617,7 +620,7 @@ gs = 3.5 [V/um]
-<div id="org6b6b451" class="figure">
+<div id="org5abdbfd" class="figure">
 <p><img src="figs/compare_Gf_id_simscape.png" alt="compare_Gf_id_simscape.png" />
 </p>
 <p><span class="figure-number">Figure 7: </span>Comparison of the identified transfer function between actuator voltage \(V_a\) and sensor stack voltage</p>
@ -626,8 +629,8 @@ gs = 3.5 [V/um]
 </div>
 </div>
-<div id="outline-container-org7cf58ef" class="outline-3">
+<div id="outline-container-org5e8b78e" class="outline-3">
-<h3 id="org7cf58ef"><span class="section-number-3">2.4</span> Conclusion</h3>
+<h3 id="org5e8b78e"><span class="section-number-3">2.4</span> Conclusion</h3>
 <div class="outline-text-3" id="text-2-4">
 <p>
 The obtained parameters \(g_a\) and \(g_s\) are not consistent between the different methods.
@ -644,11 +647,11 @@ The one using the experimental data are saved and further used.
 </div>
 </div>
-<div id="outline-container-org36bf85c" class="outline-2">
+<div id="outline-container-orgaded65a" class="outline-2">
-<h2 id="org36bf85c"><span class="section-number-2">3</span> Simscape model of the test-bench</h2>
+<h2 id="orgaded65a"><span class="section-number-2">3</span> Simscape model of the test-bench</h2>
 <div class="outline-text-2" id="text-3">
 <p>
-<a id="orgb81d61b"></a>
+<a id="org9d27452"></a>
 </p>
 <p>
 The idea here is to model the test-bench using Simscape.
@ -659,19 +662,19 @@ Whereas the suspended mass and metrology frame can be considered as rigid bodies
 </p>
 <p>
-To model the APA, a Finite Element Model (FEM) is used (Figure <a href="#orgbdafa32">8</a>) and imported into Simscape.
+To model the APA, a Finite Element Model (FEM) is used (Figure <a href="#orgf4bc6a9">8</a>) and imported into Simscape.
 </p>
-<div id="orgbdafa32" class="figure">
+<div id="orgf4bc6a9" class="figure">
 <p><img src="figs/APA95ML_FEM.png" alt="APA95ML_FEM.png" />
 </p>
 <p><span class="figure-number">Figure 8: </span>Finite Element Model of the APA95ML</p>
 </div>
 </div>
-<div id="outline-container-orgca28e7b" class="outline-3">
+<div id="outline-container-orgbd18d02" class="outline-3">
-<h3 id="orgca28e7b"><span class="section-number-3">3.1</span> Import Mass Matrix, Stiffness Matrix, and Interface Nodes Coordinates</h3>
+<h3 id="orgbd18d02"><span class="section-number-3">3.1</span> Import Mass Matrix, Stiffness Matrix, and Interface Nodes Coordinates</h3>
 <div class="outline-text-3" id="text-3-1">
 <p>
 We first extract the stiffness and mass matrices.
@ -997,7 +1000,6 @@ M = readmatrix(<span class="org-string">'APA95ML_M.CSV'</span>);
 </tbody>
 </table>
 <p>
 Then, we extract the coordinates of the interface nodes.
 </p>
@ -1007,7 +1009,7 @@ Then, we extract the coordinates of the interface nodes.
 </div>
 <p>
-The interface nodes are shown in Figure <a href="#org7dd0e69">9</a> and their coordinates are listed in Table <a href="#orgc8e88d6">4</a>.
+The interface nodes are shown in Figure <a href="#org25039e2">9</a> and their coordinates are listed in Table <a href="#orga84d9cf">4</a>.
 </p>
 <table border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
@ -1041,7 +1043,7 @@ The interface nodes are shown in Figure <a href="#org7dd0e69">9</a> and their co
 </tbody>
 </table>
-<table id="orgc8e88d6" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
+<table id="orga84d9cf" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
 <caption class="t-above"><span class="table-number">Table 4:</span> Coordinates of the interface nodes</caption>
 <colgroup>
@ -1124,7 +1126,7 @@ The interface nodes are shown in Figure <a href="#org7dd0e69">9</a> and their co
 </table>
-<div id="org7dd0e69" class="figure">
+<div id="org25039e2" class="figure">
 <p><img src="figs/APA95ML_nodes_1.png" alt="APA95ML_nodes_1.png" />
 </p>
 <p><span class="figure-number">Figure 9: </span>Interface Nodes chosen for the APA95ML</p>
@ -1136,8 +1138,8 @@ Using <code>K</code>, <code>M</code> and <code>int_xyz</code>, we can use the <c
 </div>
 </div>
-<div id="outline-container-orga49ef8e" class="outline-3">
+<div id="outline-container-org6df0376" class="outline-3">
-<h3 id="orga49ef8e"><span class="section-number-3">3.2</span> Simscape Model</h3>
+<h3 id="org6df0376"><span class="section-number-3">3.2</span> Simscape Model</h3>
 <div class="outline-text-3" id="text-3-2">
 <p>
 The flexible element is imported using the <code>Reduced Order Flexible Solid</code> Simscape block.
@ -1152,17 +1154,22 @@ A <code>Relative Motion Sensor</code> block is added between the nodes 1 and 2 t
 One mass is fixed at one end of the piezo-electric stack actuator, the other end is fixed to the world frame.
 </p>
 <div class="org-src-container">
-<pre class="src src-matlab">m = 5;
+<pre class="src src-matlab">m = 5.5;
 </pre>
 </div>
 <div class="org-src-container">
 <pre class="src src-matlab">load(<span class="org-string">'apa95ml_params.mat'</span>, <span class="org-string">'ga'</span>, <span class="org-string">'gs'</span>);
 </pre>
 </div>
 </div>
 </div>
-<div id="outline-container-orgf1f3f75" class="outline-3">
+<div id="outline-container-org1d92e82" class="outline-3">
-<h3 id="orgf1f3f75"><span class="section-number-3">3.3</span> Dynamics from Actuator Voltage to Vertical Mass Displacement</h3>
+<h3 id="org1d92e82"><span class="section-number-3">3.3</span> Dynamics from Actuator Voltage to Vertical Mass Displacement</h3>
 <div class="outline-text-3" id="text-3-3">
 <p>
-The identified dynamics is shown in Figure <a href="#orgd2ca2ca">10</a>.
+The identified dynamics is shown in Figure <a href="#org162e335">10</a>.
 </p>
 <div class="org-src-container">
@ -1174,12 +1181,12 @@ clear io; io_i = 1;
 io(io_i) = linio([mdl, <span class="org-string">'/Va'</span>], 1, <span class="org-string">'openinput'</span>);  io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Actuator Voltage [V]</span>
 io(io_i) = linio([mdl, <span class="org-string">'/y'</span>],  1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Vertical Displacement [m]</span>
-Ghm = <span class="org-type">-</span>linearize(mdl, io);
+Ghm = linearize(mdl, io);
 </pre>
 </div>
-<div id="orgd2ca2ca" class="figure">
+<div id="org162e335" class="figure">
 <p><img src="figs/dynamics_act_disp_comp_mass.png" alt="dynamics_act_disp_comp_mass.png" />
 </p>
 <p><span class="figure-number">Figure 10: </span>Dynamics from \(F\) to \(d\) without a payload and with a 5kg payload</p>
@ -1187,11 +1194,11 @@ Ghm = <span class="org-type">-</span>linearize(mdl, io);
 </div>
 </div>
-<div id="outline-container-orgd4a750c" class="outline-3">
+<div id="outline-container-org18d1e8d" class="outline-3">
-<h3 id="orgd4a750c"><span class="section-number-3">3.4</span> Dynamics from Actuator Voltage to Force Sensor Voltage</h3>
+<h3 id="org18d1e8d"><span class="section-number-3">3.4</span> Dynamics from Actuator Voltage to Force Sensor Voltage</h3>
 <div class="outline-text-3" id="text-3-4">
 <p>
-The obtained dynamics is shown in Figure <a href="#orgb135de4">11</a>.
+The obtained dynamics is shown in Figure <a href="#org2258d23">11</a>.
 </p>
 <div class="org-src-container">
@ -1208,7 +1215,7 @@ Gfm = linearize(mdl, io);
 </div>
-<div id="orgb135de4" class="figure">
+<div id="org2258d23" class="figure">
 <p><img src="figs/dynamics_force_force_sensor_comp_mass.png" alt="dynamics_force_force_sensor_comp_mass.png" />
 </p>
 <p><span class="figure-number">Figure 11: </span>Dynamics from \(F\) to \(F_m\) for \(m=0\) and \(m = 10kg\)</p>
@ -1216,34 +1223,36 @@ Gfm = linearize(mdl, io);
 </div>
 </div>
-<div id="outline-container-org1ac4d3c" class="outline-3">
+<div id="outline-container-org74335e3" class="outline-3">
-<h3 id="org1ac4d3c"><span class="section-number-3">3.5</span> Save Data for further use</h3>
+<h3 id="org74335e3"><span class="section-number-3">3.5</span> Save Data for further use</h3>
 <div class="outline-text-3" id="text-3-5">
 <div class="org-src-container">
 <pre class="src src-matlab">save(<span class="org-string">'matlab/mat/fem_simscape_models.mat'</span>, <span class="org-string">'Ghm'</span>, <span class="org-string">'Gfm'</span>)
 </pre>
 </div>
 <div class="org-src-container">
 <pre class="src src-matlab">save(<span class="org-string">'mat/fem_simscape_models.mat'</span>, <span class="org-string">'Ghm'</span>, <span class="org-string">'Gfm'</span>)
 </pre>
 </div>
 </div>
 </div>
 </div>
-<div id="outline-container-org9de639d" class="outline-2">
+<div id="outline-container-org98bd921" class="outline-2">
-<h2 id="org9de639d"><span class="section-number-2">4</span> Huddle Test</h2>
+<h2 id="org98bd921"><span class="section-number-2">4</span> Measurement of the ambient noise in the system</h2>
 <div class="outline-text-2" id="text-4">
 <p>
-<a id="org9acdf87"></a>
+<a id="orga049d7f"></a>
 </p>
 <p>
 This first measurement consist of measuring the displacement of the mass using the interferometer when no voltage is applied to the actuator.
 </p>
 <p>
 This can help determining the actuator voltage necessary to generate a motion way above the measured noise and disturbances, and thus obtain a good identification.
 </p>
 </div>
-<div id="outline-container-orgaf93c0c" class="outline-3">
+<div id="outline-container-orge831f73" class="outline-3">
-<h3 id="orgaf93c0c"><span class="section-number-3">4.1</span> Time Domain Data</h3>
+<h3 id="orge831f73"><span class="section-number-3">4.1</span> Time Domain Data</h3>
 <div class="outline-text-3" id="text-4-1">
-<div id="orgd048822" class="figure">
+<div id="org21f1e60" class="figure">
 <p><img src="figs/huddle_test_time_domain.png" alt="huddle_test_time_domain.png" />
 </p>
 <p><span class="figure-number">Figure 12: </span>Measurement of the Mass displacement during Huddle Test</p>
@ -1251,8 +1260,8 @@ Gfm = linearize(mdl, io);
 </div>
 </div>
-<div id="outline-container-org22ccdcf" class="outline-3">
+<div id="outline-container-org38d16b4" class="outline-3">
-<h3 id="org22ccdcf"><span class="section-number-3">4.2</span> PSD of Measurement Noise</h3>
+<h3 id="org38d16b4"><span class="section-number-3">4.2</span> PSD of Measurement Noise</h3>
 <div class="outline-text-3" id="text-4-2">
 <div class="org-src-container">
 <pre class="src src-matlab">Ts = t(end)<span class="org-type">/</span>(length(t)<span class="org-type">-</span>1);
@ -1268,7 +1277,7 @@ win = hanning(ceil(1<span class="org-type">*</span>Fs));
 </div>
-<div id="orgd309d3e" class="figure">
+<div id="org5264502" class="figure">
 <p><img src="figs/huddle_test_pdf.png" alt="huddle_test_pdf.png" />
 </p>
 <p><span class="figure-number">Figure 13: </span>Amplitude Spectral Density of the Displacement during Huddle Test</p>
@ -1277,24 +1286,30 @@ win = hanning(ceil(1<span class="org-type">*</span>Fs));
 </div>
 </div>
-<div id="outline-container-org914a7da" class="outline-2">
+<div id="outline-container-org90553e0" class="outline-2">
-<h2 id="org914a7da"><span class="section-number-2">5</span> Identification of the dynamics from actuator to displacement</h2>
+<h2 id="org90553e0"><span class="section-number-2">5</span> Identification of the dynamics from actuator Voltage to displacement</h2>
 <div class="outline-text-2" id="text-5">
 <p>
-<a id="org8cacbcf"></a>
+<a id="org72688d3"></a>
 </p>
 <p>
 The setup used for the identification of the dynamics from \(V_a\) to \(d\) is shown in Figure <a href="#org43a6d6f">14</a>.
 </p>
 <ul class="org-ul">
 <li class="off"><code>[&#xa0;]</code> List of equipment</li>
 <li class="off"><code>[&#xa0;]</code> Schematic</li>
 <li class="off"><code>[&#xa0;]</code> Problem of matching between the models? (there is a factor 10)</li>
 </ul>
 <p>
-E505 with gain of 10.
+A Voltage amplifier with a gain of \(10\) is used.
 Two stacks are used as actuators while one stack is used as a force sensor.
 </p>
 <div id="org43a6d6f" class="figure">
 <p><img src="figs/test_bench_apa_identification.png" alt="test_bench_apa_identification.png" />
 </p>
 <p><span class="figure-number">Figure 14: </span>Test Bench used for the identification of the dynaimcs from \(V_a\) to \(d\)</p>
 </div>
-<div id="outline-container-orga1aace1" class="outline-3">
+</div>
-<h3 id="orga1aace1"><span class="section-number-3">5.1</span> Load Data</h3>
+<div id="outline-container-orgbdf7a7b" class="outline-3">
 <h3 id="orgbdf7a7b"><span class="section-number-3">5.1</span> Load Data</h3>
 <div class="outline-text-3" id="text-5-1">
 <p>
 The data from the &ldquo;noise test&rdquo; and the identification test are loaded.
@ -1322,14 +1337,13 @@ Now we add a factor 10 to take into account the gain of the voltage amplifier.
 </p>
 <div class="org-src-container">
 <pre class="src src-matlab">um   = 10<span class="org-type">*</span>um;
 ht.u = 10<span class="org-type">*</span>ht.u;
 </pre>
 </div>
 </div>
 </div>
-<div id="outline-container-org1284757" class="outline-3">
+<div id="outline-container-org5513479" class="outline-3">
-<h3 id="org1284757"><span class="section-number-3">5.2</span> Comparison of the PSD with Huddle Test</h3>
+<h3 id="org5513479"><span class="section-number-3">5.2</span> Comparison of the PSD with Huddle Test</h3>
 <div class="outline-text-3" id="text-5-2">
 <div class="org-src-container">
 <pre class="src src-matlab">Ts = t(end)<span class="org-type">/</span>(length(t)<span class="org-type">-</span>1);
@ -1346,16 +1360,16 @@ win = hanning(ceil(1<span class="org-type">*</span>Fs));
 </div>
-<div id="org87d1b81" class="figure">
+<div id="orgf5a2201" class="figure">
 <p><img src="figs/apa95ml_5kg_PI_pdf_comp_huddle.png" alt="apa95ml_5kg_PI_pdf_comp_huddle.png" />
 </p>
-<p><span class="figure-number">Figure 14: </span>Comparison of the ASD for the identification test and the huddle test</p>
+<p><span class="figure-number">Figure 15: </span>Comparison of the ASD for the identification test and the huddle test</p>
 </div>
 </div>
 </div>
-<div id="outline-container-org89453e3" class="outline-3">
+<div id="outline-container-org4d4bebc" class="outline-3">
-<h3 id="org89453e3"><span class="section-number-3">5.3</span> Compute TF estimate and Coherence</h3>
+<h3 id="org4d4bebc"><span class="section-number-3">5.3</span> Compute TF estimate and Coherence</h3>
 <div class="outline-text-3" id="text-5-3">
 <div class="org-src-container">
 <pre class="src src-matlab">[tf_est, f] = tfestimate(um, <span class="org-type">-</span>y, win, [], [], 1<span class="org-type">/</span>Ts);
@ -1364,10 +1378,10 @@ win = hanning(ceil(1<span class="org-type">*</span>Fs));
 </div>
-<div id="org69f52ac" class="figure">
+<div id="org23ac087" class="figure">
 <p><img src="figs/apa95ml_5kg_PI_coh.png" alt="apa95ml_5kg_PI_coh.png" />
 </p>
-<p><span class="figure-number">Figure 15: </span>Coherence</p>
+<p><span class="figure-number">Figure 16: </span>Coherence</p>
 </div>
 <p>
@ -1379,31 +1393,23 @@ Comparison with the FEM model
 </div>
-<div id="orgac63fe3" class="figure">
+<div id="orged73e47" class="figure">
 <p><img src="figs/apa95ml_5kg_pi_comp_fem.png" alt="apa95ml_5kg_pi_comp_fem.png" />
 </p>
-<p><span class="figure-number">Figure 16: </span>Comparison of the identified transfer function and the one estimated from the FE model</p>
+<p><span class="figure-number">Figure 17: </span>Comparison of the identified transfer function and the one estimated from the FE model</p>
 </div>
 </div>
 </div>
 </div>
-<div id="outline-container-orgfff18d2" class="outline-2">
+<div id="outline-container-org12553c5" class="outline-2">
-<h2 id="orgfff18d2"><span class="section-number-2">6</span> Identification of the dynamics from actuator to force sensor</h2>
+<h2 id="org12553c5"><span class="section-number-2">6</span> Identification of the dynamics from actuator Voltage to force sensor Voltage</h2>
 <div class="outline-text-2" id="text-6">
 <p>
-<a id="org2eda6f2"></a>
+<a id="orgdd284b9"></a>
 </p>
 <p>
-Two measurements are performed:
+The same setup shown in Figure <a href="#org43a6d6f">14</a> is used.
 </p>
 <ul class="org-ul">
 <li>Speedgoat DAC =&gt; Voltage Amplifier (x20) =&gt; 1 Piezo Stack =&gt; &#x2026; =&gt; 2 Stacks as Force Sensor (parallel) =&gt; Speedgoat ADC</li>
 <li>Speedgoat DAC =&gt; Voltage Amplifier (x20) =&gt; 2 Piezo Stacks (parallel) =&gt; &#x2026; =&gt; 1 Stack as Force Sensor =&gt; Speedgoat ADC</li>
 </ul>
 <p>
 The obtained dynamics from force actuator to force sensor are compare with the FEM model.
 </p>
 <p>
 The data are loaded:
@ -1413,76 +1419,88 @@ The data are loaded:
 </pre>
 </div>
 <div class="org-src-container">
 <pre class="src src-matlab">u = detrend(u, 0);
 v = detrend(v, 0);
 </pre>
 </div>
 <div class="org-src-container">
 <pre class="src src-matlab">u = 20<span class="org-type">*</span>u;
 </pre>
 </div>
 <p>
-Let&rsquo;s use the amplifier gain to obtain the true voltage applied to the actuator stack(s)
+Any offset is removed.
 </p>
 <p>
 The parameters of the piezoelectric stacks are defined below:
 </p>
 <div class="org-src-container">
-<pre class="src src-matlab">d33 = 3e<span class="org-type">-</span>10; <span class="org-comment">% Strain constant [m/V]</span>
+<pre class="src src-matlab">u = detrend(u, 0); <span class="org-comment">% Speedgoat DAC output Voltage [V]</span>
-n = 80; <span class="org-comment">% Number of layers per stack</span>
+v = detrend(v, 0); <span class="org-comment">% Speedgoat ADC input Voltage (sensor stack) [V]</span>
 eT = 1.6e<span class="org-type">-</span>8; <span class="org-comment">% Permittivity under constant stress [F/m]</span>
 sD = 2e<span class="org-type">-</span>11; <span class="org-comment">% Elastic compliance under constant electric displacement [m2/N]</span>
 ka = 235e6; <span class="org-comment">% Stack stiffness [N/m]</span>
 </pre>
 </div>
 <p>
-From the FEM, we construct the transfer function from DAC voltage to ADC voltage.
+Here, the amplifier gain is 20.
 </p>
 <div class="org-src-container">
-<pre class="src src-matlab">Gfem_aa_s = exp(<span class="org-type">-</span>s<span class="org-type">/</span>1e4)<span class="org-type">*</span>20<span class="org-type">*</span>(2<span class="org-type">*</span>d33<span class="org-type">*</span>n<span class="org-type">*</span>ka)<span class="org-type">*</span>(G(3,1)<span class="org-type">+</span>G(3,2))<span class="org-type">*</span>d33<span class="org-type">/</span>(eT<span class="org-type">*</span>sD<span class="org-type">*</span>n);
+<pre class="src src-matlab">u = 20<span class="org-type">*</span>u; <span class="org-comment">% Actuator Stack Voltage [V]</span>
 Gfem_a_ss = exp(<span class="org-type">-</span>s<span class="org-type">/</span>1e4)<span class="org-type">*</span>20<span class="org-type">*</span>(  d33<span class="org-type">*</span>n<span class="org-type">*</span>ka)<span class="org-type">*</span>(G(3,1)<span class="org-type">+</span>G(2,1))<span class="org-type">*</span>d33<span class="org-type">/</span>(eT<span class="org-type">*</span>sD<span class="org-type">*</span>n);
 </pre>
 </div>
 <div class="org-src-container">
 <pre class="src src-matlab">Gfem_aa_s = exp(<span class="org-type">-</span>s<span class="org-type">/</span>1e4)<span class="org-type">*</span>20<span class="org-type">*</span>(2<span class="org-type">*</span>d33<span class="org-type">*</span>n<span class="org-type">*</span>ka)<span class="org-type">*</span>Gfm<span class="org-type">*</span>d33<span class="org-type">/</span>(eT<span class="org-type">*</span>sD<span class="org-type">*</span>n);
 Gfem_a_ss = exp(<span class="org-type">-</span>s<span class="org-type">/</span>1e4)<span class="org-type">*</span>20<span class="org-type">*</span>(  d33<span class="org-type">*</span>n<span class="org-type">*</span>ka)<span class="org-type">*</span>Gfm<span class="org-type">*</span>d33<span class="org-type">/</span>(eT<span class="org-type">*</span>sD<span class="org-type">*</span>n);
 </pre>
 </div>
 <p>
-The transfer function from input voltage to output voltage are computed and shown in Figure <a href="#org157bd9b">17</a>.
+The transfer function from the actuator voltage \(V_a\) to the force sensor stack voltage \(V_s\) is computed.
 </p>
 <div class="org-src-container">
 <pre class="src src-matlab">Ts = t(end)<span class="org-type">/</span>(length(t)<span class="org-type">-</span>1);
 Fs = 1<span class="org-type">/</span>Ts;
-win = hann(ceil(10<span class="org-type">/</span>Ts));
+win = hann(ceil(5<span class="org-type">/</span>Ts));
 [tf_est,  f] = tfestimate(u, v, win, [], [], 1<span class="org-type">/</span>Ts);
 [coh,     <span class="org-type">~</span>] = mscohere(  u, v, win, [], [], 1<span class="org-type">/</span>Ts);
 </pre>
 </div>
 <p>
 The coherence is shown in Figure <a href="#org0c44552">18</a>.
 </p>
 <div id="org0c44552" class="figure">
 <p><img src="figs/apa95ml_5kg_cedrat_coh.png" alt="apa95ml_5kg_cedrat_coh.png" />
 </p>
 <p><span class="figure-number">Figure 18: </span>Coherence</p>
 </div>
 <p>
 The Simscape model is loaded and compared with the identified dynamics in Figure <a href="#org5ad15cc">19</a>.
 The non-minimum phase zero is just a side effect of the not so great identification.
 Taking longer measurements would results in a minimum phase zero.
 </p>
 <div class="org-src-container">
 <pre class="src src-matlab">load(<span class="org-string">'mat/fem_simscape_models.mat'</span>, <span class="org-string">'Gfm'</span>);
 </pre>
 </div>
-<div id="org157bd9b" class="figure">
+<div id="org5ad15cc" class="figure">
 <p><img src="figs/bode_plot_force_sensor_voltage_comp_fem.png" alt="bode_plot_force_sensor_voltage_comp_fem.png" />
 </p>
-<p><span class="figure-number">Figure 17: </span>Comparison of the identified dynamics from voltage output to voltage input and the FEM</p>
+<p><span class="figure-number">Figure 19: </span>Comparison of the identified dynamics from voltage output to voltage input and the FEM</p>
 </div>
 </div>
-<div id="outline-container-orgfa2171a" class="outline-3">
+</div>
-<h3 id="orgfa2171a"><span class="section-number-3">6.1</span> System Identification</h3>
+
-<div class="outline-text-3" id="text-6-1">
+<div id="outline-container-org6f69286" class="outline-2">
 <h2 id="org6f69286"><span class="section-number-2">7</span> Integral Force Feedback</h2>
 <div class="outline-text-2" id="text-7">
 <p>
 <a id="org13dae17"></a>
 </p>
 <div id="org5ae9700" class="figure">
 <p><img src="figs/test_bench_apa_schematic_iff.png" alt="test_bench_apa_schematic_iff.png" />
 </p>
 <p><span class="figure-number">Figure 20: </span>Schematic of the test bench using IFF</p>
 </div>
 </div>
 <div id="outline-container-orgbc62cba" class="outline-3">
 <h3 id="orgbc62cba"><span class="section-number-3">7.1</span> IFF Plant</h3>
 <div class="outline-text-3" id="text-7-1">
 <p>
 From the identified plant, a model of the transfer function from the actuator stack voltage to the force sensor generated voltage is developed.
 </p>
 <div class="org-src-container">
 <pre class="src src-matlab">w_z = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>111; <span class="org-comment">% Zeros frequency [rad/s]</span>
 w_p = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>255; <span class="org-comment">% Pole frequency [rad/s]</span>
@ -1490,50 +1508,63 @@ xi_z = 0.05;
 xi_p = 0.015;
 G_inf = 0.1;
-Gi = G_inf<span class="org-type">*</span>(s<span class="org-type">^</span>2 <span class="org-type">-</span> 2<span class="org-type">*</span>xi_z<span class="org-type">*</span>w_z<span class="org-type">*</span>s <span class="org-type">+</span> w_z<span class="org-type">^</span>2)<span class="org-type">/</span>(s<span class="org-type">^</span>2 <span class="org-type">+</span> 2<span class="org-type">*</span>xi_p<span class="org-type">*</span>w_p<span class="org-type">*</span>s <span class="org-type">+</span> w_p<span class="org-type">^</span>2);
+Gi = G_inf<span class="org-type">*</span>(s<span class="org-type">^</span>2 <span class="org-type">+</span> 2<span class="org-type">*</span>xi_z<span class="org-type">*</span>w_z<span class="org-type">*</span>s <span class="org-type">+</span> w_z<span class="org-type">^</span>2)<span class="org-type">/</span>(s<span class="org-type">^</span>2 <span class="org-type">+</span> 2<span class="org-type">*</span>xi_p<span class="org-type">*</span>w_p<span class="org-type">*</span>s <span class="org-type">+</span> w_p<span class="org-type">^</span>2);
 </pre>
 </div>
 <p>
 Its bode plot is shown in Figure <a href="#org255cfc1">21</a>.
 </p>
-<div id="orgf552545" class="figure">
+
 <div id="org255cfc1" class="figure">
 <p><img src="figs/iff_plant_identification_apa95ml.png" alt="iff_plant_identification_apa95ml.png" />
 </p>
-<p><span class="figure-number">Figure 18: </span>Identification of the IFF plant</p>
+<p><span class="figure-number">Figure 21: </span>Bode plot of the IFF plant</p>
 </div>
 </div>
 </div>
 <p>
 The controller used in the Integral Force Feedback Architecture is:
 </p>
 \begin{equation}
  K_{\text{IFF}}(s) = \frac{g}{s + 2\cdot 2\pi} \cdot \frac{s}{s + 0.5 \cdot 2\pi}
 \end{equation}
 <p>
 where \(g\) is a gain that can be tuned.
 </p>
-<div id="outline-container-org633e9f1" class="outline-3">
+<p>
-<h3 id="org633e9f1"><span class="section-number-3">6.2</span> Integral Force Feedback</h3>
+Above 2 Hz the controller is basically an integrator, whereas an high pass filter is added at 0.5Hz to further reduce the low frequency gain.
-<div class="outline-text-3" id="text-6-2">
+</p>
-<div id="orgca6756f" class="figure">
+<p>
 In the frequency band of interest, this controller should mostly act as a pure integrator.
 </p>
 <p>
 The Root Locus corresponding to this controller is shown in Figure <a href="#org70704b4">22</a>.
 </p>
 <div id="org70704b4" class="figure">
 <p><img src="figs/root_locus_iff_apa95ml_identification.png" alt="root_locus_iff_apa95ml_identification.png" />
 </p>
-<p><span class="figure-number">Figure 19: </span>Root Locus for IFF</p>
+<p><span class="figure-number">Figure 22: </span>Root Locus for IFF</p>
 </div>
 </div>
 </div>
 </div>
-<div id="outline-container-org1bf58e2" class="outline-2">
+
-<h2 id="org1bf58e2"><span class="section-number-2">7</span> Integral Force Feedback</h2>
+<div id="outline-container-org4d62e16" class="outline-3">
-<div class="outline-text-2" id="text-7">
+<h3 id="org4d62e16"><span class="section-number-3">7.2</span> First tests with few gains</h3>
 <div class="outline-text-3" id="text-7-2">
 <p>
-<a id="org6db5225"></a>
+The controller is now implemented in practice, and few controller gains are tested: \(g = 0\), \(g = 10\) and \(g = 100\).
 </p>
-<div id="org86a6667" class="figure">
+<p>
-<p><img src="figs/test_bench_apa_schematic_iff.png" alt="test_bench_apa_schematic_iff.png" />
+For each controller gain, the identification shown in Figure <a href="#org5ae9700">20</a> is performed.
 </p>
 <p><span class="figure-number">Figure 20: </span>Schematic of the test bench using IFF</p>
 </div>
 </div>
 <div id="outline-container-org25092d8" class="outline-3">
 <h3 id="org25092d8"><span class="section-number-3">7.1</span> First tests with few gains</h3>
 <div class="outline-text-3" id="text-7-1">
 <div class="org-src-container">
 <pre class="src src-matlab">iff_g10  = load(<span class="org-string">'apa95ml_iff_g10_res.mat'</span>,  <span class="org-string">'u'</span>, <span class="org-string">'t'</span>, <span class="org-string">'y'</span>, <span class="org-string">'v'</span>);
 iff_g100 = load(<span class="org-string">'apa95ml_iff_g100_res.mat'</span>, <span class="org-string">'u'</span>, <span class="org-string">'t'</span>, <span class="org-string">'y'</span>, <span class="org-string">'v'</span>);
@ -1556,26 +1587,37 @@ win = hann(ceil(10<span class="org-type">/</span>Ts));
 </pre>
 </div>
 <p>
 The coherence between the excitation signal and the mass displacement as measured by the interferometer is shown in Figure <a href="#orga102681">23</a>.
 </p>
-<div id="org2da6c81" class="figure">
+
 <div id="orga102681" class="figure">
 <p><img src="figs/iff_first_test_coherence.png" alt="iff_first_test_coherence.png" />
 </p>
-<p><span class="figure-number">Figure 21: </span>Coherence</p>
+<p><span class="figure-number">Figure 23: </span>Coherence</p>
 </div>
 <p>
 The obtained transfer functions are shown in Figure <a href="#org64a82bf">24</a>.
 It is clear that the IFF architecture can actively damp the main resonance of the system.
 </p>
-<div id="org407e7b6" class="figure">
+<div id="org64a82bf" class="figure">
 <p><img src="figs/iff_first_test_bode_plot.png" alt="iff_first_test_bode_plot.png" />
 </p>
-<p><span class="figure-number">Figure 22: </span>Bode plot for different values of IFF gain</p>
+<p><span class="figure-number">Figure 24: </span>Bode plot for different values of IFF gain</p>
 </div>
 </div>
 </div>
-<div id="outline-container-org67fa466" class="outline-3">
+<div id="outline-container-org4304a4e" class="outline-3">
-<h3 id="org67fa466"><span class="section-number-3">7.2</span> Second test with many Gains</h3>
+<h3 id="org4304a4e"><span class="section-number-3">7.3</span> Second test with many Gains</h3>
-<div class="outline-text-3" id="text-7-2">
+<div class="outline-text-3" id="text-7-3">
 <p>
 Then, the same identification test is performed for many more gains.
 </p>
 <div class="org-src-container">
 <pre class="src src-matlab">load(<span class="org-string">'apa95ml_iff_test.mat'</span>, <span class="org-string">'results'</span>);
 </pre>
@ -1602,12 +1644,21 @@ g_iff = [0, 1, 5, 10, 50, 100];
 </pre>
 </div>
 <p>
 The obtained dynamics are shown in Figure <a href="#org04c2472">25</a>.
 </p>
-<div id="orgfa8bc3c" class="figure">
+
 <div id="org04c2472" class="figure">
 <p><img src="figs/iff_results_bode_plots.png" alt="iff_results_bode_plots.png" />
 </p>
 <p><span class="figure-number">Figure 25: </span>Identified dynamics from excitation voltage to the mass displacement</p>
 </div>
 <p>
 For each gain, the parameters of a second order resonant system that best fits the data are estimated and are compared with the data in Figure <a href="#org21dc325">26</a>.
 </p>
 <div class="org-src-container">
 <pre class="src src-matlab">G_id = {zeros(1,length(results))};
@ -1625,15 +1676,21 @@ f_end = 500; <span class="org-comment">% [Hz]</span>
 </div>
-<div id="org62b4b70" class="figure">
+<div id="org21dc325" class="figure">
 <p><img src="figs/iff_results_bode_plots_identification.png" alt="iff_results_bode_plots_identification.png" />
 </p>
 <p><span class="figure-number">Figure 26: </span>Comparison of the measured dynamic and the identified 2nd order resonant systems that best fits the data</p>
 </div>
 <p>
 Finally, we can represent the position of the poles of the 2nd order systems on the Root Locus plot (Figure <a href="#orgd96aad3">27</a>).
 </p>
-<div id="org49a571f" class="figure">
+
 <div id="orgd96aad3" class="figure">
 <p><img src="figs/iff_results_root_locus.png" alt="iff_results_root_locus.png" />
 </p>
 <p><span class="figure-number">Figure 27: </span>Root Locus plot of the identified IFF plant as well as the identified poles of the damped system</p>
 </div>
 <style>.csl-entry{text-indent: -1.5em; margin-left: 1.5em;}</style><h2 class='citeproc-org-bib-h2'>Bibliography</h2>
@ -1646,7 +1703,7 @@ f_end = 500; <span class="org-comment">% [Hz]</span>
 </div>
 <div id="postamble" class="status">
 <p class="author">Author: Dehaeze Thomas</p>
-<p class="date">Created: 2020-11-24 mar. 13:54</p>
+<p class="date">Created: 2020-11-24 mar. 18:24</p>
 </div>
 </body>
 </html>
--- a/index.org
+++ b/index.org
@ -28,11 +28,14 @@
 * Introduction                                                        :ignore:
- Section [[sec:experimental_setup]]:
+This document is divided in the following sections:
- Section [[sec:simscape_model]]:
+- Section [[sec:experimental_setup]]: the experimental setup is described
- Section [[]]:
+- Section [[sec:estimation_piezo_params]]: the parameters which are important for the Simscape model of the piezoelectric stack actuator and sensors are estimated
- Section [[]]:
+- Section [[sec:simscape_model]]: the Simscape model of the test bench is presented
- Section [[]]:
+- Section [[sec:huddle_test]]: as usual, a first measurement of the noise/disturbances present in the system is performed
 - Section [[sec:motion_identification]]: the transfer function from the actuator voltage to the displacement of the mass is identified and compared with the model
 - Section [[sec:force_sensor_identification]]: the tranfer function from the actuator voltage to the sensor stack voltage is identified and compare with the model
 - Section [[sec:integral_force_feedback]]: the Integral Force Feedback control architecture is applied on the system using the force sensor stack in order to add damping to the suspension resonance
 * Experimental Setup
 <<sec:experimental_setup>>
@ -68,6 +71,11 @@ Here are the equipment used in the test bench:
 #+end_note
 * Estimation of electrical/mechanical relationships
 :PROPERTIES:
 :header-args:matlab+: :tangle matlab/piezo_parameters.m
 :header-args:matlab+: :comments org :mkdirp yes
 :END:
 <<sec:estimation_piezo_params>>
 ** Introduction                                                      :ignore:
 In order to correctly model the piezoelectric actuator with Simscape, we need to determine:
 1. $g_a$: the ratio of the generated force $F_a$ to the supply voltage $V_a$ across the piezoelectric stack
@ -130,11 +138,15 @@ The relation between the applied voltage and the generated force can be estimate
 #+end_src
 #+begin_src matlab :results value replace
-  ka*Lmax/Vmax % [N/V]
+  ga = ka*Lmax/Vmax; % [N/V]
 #+end_src
 #+begin_src matlab :results value replace :exports results :tangle no
  sprintf('ga = %.1f [N/V]', ga)
 #+end_src
 #+RESULTS:
-: 27.647
+: ga = 27.6 [N/V]
 From the parameters of the stack, it seems not possible to estimate the relation between the strain and the generated voltage.
@ -564,7 +576,6 @@ We first extract the stiffness and mass matrices.
 |   6e-07 |  -4e-08 | 0.0003 | -3e-10 |  -2e-09 | -2e-09 | -2e-06 |  -9e-07 |   0.02 |  2e-08 |
 |  -8e-09 |   5e-06 |  1e-09 |  3e-08 |   7e-11 |  2e-13 |  8e-09 |  -9e-05 |  2e-08 |  1e-06 |
 Then, we extract the coordinates of the interface nodes.
 #+begin_src matlab
  [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA95ML_out_nodes_3D.txt');
@ -613,7 +624,11 @@ A =Relative Motion Sensor= block is added between the nodes 1 and 2 to measure t
 One mass is fixed at one end of the piezo-electric stack actuator, the other end is fixed to the world frame.
 #+begin_src matlab
-  m = 5;
+  m = 5.5;
 #+end_src
 #+begin_src matlab
  load('apa95ml_params.mat', 'ga', 'gs');
 #+end_src
 ** Dynamics from Actuator Voltage to Vertical Mass Displacement
@ -628,7 +643,7 @@ The identified dynamics is shown in Figure [[fig:dynamics_act_disp_comp_mass]].
  io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1; % Actuator Voltage [V]
  io(io_i) = linio([mdl, '/y'],  1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
-  Ghm = -linearize(mdl, io);
+  Ghm = linearize(mdl, io);
 #+end_src
 #+begin_src matlab :exports none
@ -642,7 +657,7 @@ The identified dynamics is shown in Figure [[fig:dynamics_act_disp_comp_mass]].
  plot(freqs, abs(squeeze(freqresp(Ghm, freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
+  ylabel('Amplitude $d/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
  hold off;
  ax2 = nexttile;
@ -650,7 +665,6 @@ The identified dynamics is shown in Figure [[fig:dynamics_act_disp_comp_mass]].
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Ghm, freqs, 'Hz')))));
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-360 0]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
@ -692,15 +706,14 @@ The obtained dynamics is shown in Figure [[fig:dynamics_force_force_sensor_comp_
  plot(freqs, abs(squeeze(freqresp(Gfm, freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
+  ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
  hold off;
  ax2 = nexttile;
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gfm, freqs, 'Hz')))));
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  yticks(-360:90:360);
+  yticks(-360:90:360); ylim([-360, 180]);
  ylim([-390 30]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
@ -721,17 +734,21 @@ The obtained dynamics is shown in Figure [[fig:dynamics_force_force_sensor_comp_
  save('matlab/mat/fem_simscape_models.mat', 'Ghm', 'Gfm')
 #+end_src
-#+begin_src matlab :eval no
+#+begin_src matlab :exports none :eval no
  save('mat/fem_simscape_models.mat', 'Ghm', 'Gfm')
 #+end_src
-* Huddle Test
+* Measurement of the ambient noise in the system
 :PROPERTIES:
 :header-args:matlab+: :tangle matlab/huddle_test.m
 :header-args:matlab+: :comments org :mkdirp yes
 :END:
 <<sec:huddle_test>>
 ** Introduction                                                      :ignore:
 This first measurement consist of measuring the displacement of the mass using the interferometer when no voltage is applied to the actuator.
 This can help determining the actuator voltage necessary to generate a motion way above the measured noise and disturbances, and thus obtain a good identification.
 ** Matlab Init                                             :noexport:ignore:
 #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
@ -803,7 +820,7 @@ The obtained dynamics is shown in Figure [[fig:dynamics_force_force_sensor_comp_
 #+RESULTS:
 [[file:figs/huddle_test_pdf.png]]
-* TODO Identification of the dynamics from actuator to displacement
+* Identification of the dynamics from actuator Voltage to displacement
 :PROPERTIES:
 :header-args:matlab+: :tangle matlab/motion_identification.m
 :header-args:matlab+: :comments org :mkdirp yes
@ -811,11 +828,14 @@ The obtained dynamics is shown in Figure [[fig:dynamics_force_force_sensor_comp_
 <<sec:motion_identification>>
 ** Introduction                                                      :ignore:
- [ ] List of equipment
+The setup used for the identification of the dynamics from $V_a$ to $d$ is shown in Figure [[fig:test_bench_apa_identification]].
 - [ ] Schematic
 - [ ] Problem of matching between the models? (there is a factor 10)
-E505 with gain of 10.
+A Voltage amplifier with a gain of $10$ is used.
 Two stacks are used as actuators while one stack is used as a force sensor.
 #+name: fig:test_bench_apa_identification
 #+caption: Test Bench used for the identification of the dynaimcs from $V_a$ to $d$
 [[file:figs/test_bench_apa_identification.png]]
 ** Matlab Init                                             :noexport:ignore:
 #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
@ -837,23 +857,21 @@ E505 with gain of 10.
 ** Load Data
 The data from the "noise test" and the identification test are loaded.
 #+begin_src matlab
-  ht = load('huddle_test.mat', 't', 'u', 'y');
+  ht = load('huddle_test.mat', 't', 'y');
  load('apa95ml_5kg_Amp_E505.mat', 't', 'um', 'y');
 #+end_src
 Any offset value is removed:
 #+begin_src matlab
-  um = detrend(um, 0); % Input Voltage [V]
+  um = detrend(um, 0); % Generated DAC Voltage [V]
  y  = detrend(y , 0); % Mass displacement [m]
  ht.u  = detrend(ht.u, 0);
  ht.y  = detrend(ht.y, 0);
 #+end_src
 Now we add a factor 10 to take into account the gain of the voltage amplifier.
 #+begin_src matlab
-  um   = 10*um;
+  Va = 10*um;
  ht.u = 10*ht.u;
 #+end_src
 ** Comparison of the PSD with Huddle Test
@ -892,8 +910,8 @@ Now we add a factor 10 to take into account the gain of the voltage amplifier.
 ** Compute TF estimate and Coherence
 #+begin_src matlab
-  [tf_est, f] = tfestimate(um, -y, win, [], [], 1/Ts);
+  [tf_est, f] = tfestimate(Va, -y, win, [], [], 1/Ts);
-  [co_est, ~] = mscohere(  um, -y, win, [], [], 1/Ts);
+  [co_est, ~] = mscohere(  Va, -y, win, [], [], 1/Ts);
 #+end_src
 #+begin_src matlab :exports none
@ -951,7 +969,7 @@ Comparison with the FEM model
 #+end_src
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/apa95ml_5kg_pi_comp_fem.pdf', 'width', 'wide', 'height', 'normal');
+  exportFig('figs/apa95ml_5kg_pi_comp_fem.pdf', 'width', 'wide', 'height', 'tall');
 #+end_src
 #+name: fig:apa95ml_5kg_pi_comp_fem
@ -959,18 +977,14 @@ Comparison with the FEM model
 #+RESULTS:
 [[file:figs/apa95ml_5kg_pi_comp_fem.png]]
-* Identification of the dynamics from actuator to force sensor
+* Identification of the dynamics from actuator Voltage to force sensor Voltage
 :PROPERTIES:
 :header-args:matlab+: :tangle matlab/force_sensor_identification.m
 :header-args:matlab+: :comments org :mkdirp yes
 :END:
 <<sec:force_sensor_identification>>
 ** Introduction                                                      :ignore:
-Two measurements are performed:
+The same setup shown in Figure [[fig:test_bench_apa_identification]] is used.
 - Speedgoat DAC => Voltage Amplifier (x20) => 1 Piezo Stack => ... => 2 Stacks as Force Sensor (parallel) => Speedgoat ADC
 - Speedgoat DAC => Voltage Amplifier (x20) => 2 Piezo Stacks (parallel) => ... => 1 Stack as Force Sensor => Speedgoat ADC
 The obtained dynamics from force actuator to force sensor are compare with the FEM model.
 ** Matlab Init                                              :noexport:ignore:
 #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
@ -995,51 +1009,56 @@ The data are loaded:
  load('apa95ml_5kg_2a_1s.mat', 't', 'u', 'v');
 #+end_src
 Any offset is removed.
 #+begin_src matlab
-  u = detrend(u, 0);
+  u = detrend(u, 0); % Speedgoat DAC output Voltage [V]
-  v = detrend(v, 0);
+  v = detrend(v, 0); % Speedgoat ADC input Voltage (sensor stack) [V]
 #+end_src
 Here, the amplifier gain is 20.
 #+begin_src matlab
-  u = 20*u;
+  u = 20*u; % Actuator Stack Voltage [V]
 #+end_src
 ** Adjust gain                                                       :ignore:
 Let's use the amplifier gain to obtain the true voltage applied to the actuator stack(s)
 The parameters of the piezoelectric stacks are defined below:
 #+begin_src matlab
  d33 = 3e-10; % Strain constant [m/V]
  n = 80; % Number of layers per stack
  eT = 1.6e-8; % Permittivity under constant stress [F/m]
  sD = 2e-11; % Elastic compliance under constant electric displacement [m2/N]
  ka = 235e6; % Stack stiffness [N/m]
 #+end_src
 From the FEM, we construct the transfer function from DAC voltage to ADC voltage.
 #+begin_src matlab
  Gfem_aa_s = exp(-s/1e4)*20*(2*d33*n*ka)*(G(3,1)+G(3,2))*d33/(eT*sD*n);
  Gfem_a_ss = exp(-s/1e4)*20*(  d33*n*ka)*(G(3,1)+G(2,1))*d33/(eT*sD*n);
 #+end_src
 #+begin_src matlab
  Gfem_aa_s = exp(-s/1e4)*20*(2*d33*n*ka)*Gfm*d33/(eT*sD*n);
  Gfem_a_ss = exp(-s/1e4)*20*(  d33*n*ka)*Gfm*d33/(eT*sD*n);
 #+end_src
 ** Compute TF estimate and Coherence                                 :ignore:
-The transfer function from input voltage to output voltage are computed and shown in Figure [[fig:bode_plot_force_sensor_voltage_comp_fem]].
+The transfer function from the actuator voltage $V_a$ to the force sensor stack voltage $V_s$ is computed.
 #+begin_src matlab
  Ts = t(end)/(length(t)-1);
  Fs = 1/Ts;
-  win = hann(ceil(10/Ts));
+  win = hann(ceil(5/Ts));
  [tf_est,  f] = tfestimate(u, v, win, [], [], 1/Ts);
  [coh,     ~] = mscohere(  u, v, win, [], [], 1/Ts);
 #+end_src
 The coherence is shown in Figure [[fig:apa95ml_5kg_cedrat_coh]].
 #+begin_src matlab :exports none
  figure;
  hold on;
  plot(f, coh, 'k-')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
  ylabel('Coherence'); xlabel('Frequency [Hz]');
  hold off;
  xlim([10, 5e3]);
 #+end_src
 #+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/apa95ml_5kg_cedrat_coh.pdf', 'width', 'wide', 'height', 'normal');
 #+end_src
 #+name: fig:apa95ml_5kg_cedrat_coh
 #+caption: Coherence
 #+RESULTS:
 [[file:figs/apa95ml_5kg_cedrat_coh.png]]
 The Simscape model is loaded and compared with the identified dynamics in Figure [[fig:bode_plot_force_sensor_voltage_comp_fem]].
 The non-minimum phase zero is just a side effect of the not so great identification.
 Taking longer measurements would results in a minimum phase zero.
 #+begin_src matlab
  load('mat/fem_simscape_models.mat', 'Gfm');
 #+end_src
@ -1055,7 +1074,7 @@ The transfer function from input voltage to output voltage are computed and show
  plot(f, abs(tf_est))
  plot(freqs, abs(squeeze(freqresp(Gfm, freqs, 'Hz'))))
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
-  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
+  ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
  hold off;
  ylim([1e-3, 1e1]);
@ -1085,7 +1104,40 @@ The transfer function from input voltage to output voltage are computed and show
 #+RESULTS:
 [[file:figs/bode_plot_force_sensor_voltage_comp_fem.png]]
-** System Identification
+* Integral Force Feedback
 :PROPERTIES:
 :header-args:matlab+: :tangle matlab/integral_force_feedback.m
 :header-args:matlab+: :comments org :mkdirp yes
 :END:
 <<sec:integral_force_feedback>>
 ** Introduction                                                      :ignore:
 The test bench used to try the Integral Force Feedback control architecture is shown in Figure [[fig:test_bench_apa_schematic_iff]].
 #+name: fig:test_bench_apa_schematic_iff
 #+caption: Schematic of the test bench using IFF
 [[file:figs/test_bench_apa_schematic_iff.png]]
 ** Matlab Init                                             :noexport:ignore:
 #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
 #+end_src
 #+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
 #+end_src
 #+begin_src matlab :tangle no
  addpath('./matlab/mat/');
 #+end_src
 #+begin_src matlab :eval no
  addpath('./mat/');
 #+end_src
 ** IFF Plant
 From the identified plant, a model of the transfer function from the actuator stack voltage to the force sensor generated voltage is developed.
 #+begin_src matlab
  w_z = 2*pi*111; % Zeros frequency [rad/s]
  w_p = 2*pi*255; % Pole frequency [rad/s]
@ -1093,9 +1145,11 @@ The transfer function from input voltage to output voltage are computed and show
  xi_p = 0.015;
  G_inf = 0.1;
-  Gi = G_inf*(s^2 - 2*xi_z*w_z*s + w_z^2)/(s^2 + 2*xi_p*w_p*s + w_p^2);
+  Gi = G_inf*(s^2 + 2*xi_z*w_z*s + w_z^2)/(s^2 + 2*xi_p*w_p*s + w_p^2);
 #+end_src
 Its bode plot is shown in Figure [[fig:iff_plant_identification_apa95ml]].
 #+begin_src matlab :exports none
  freqs = logspace(1, 4, 1000);
@ -1104,8 +1158,7 @@ The transfer function from input voltage to output voltage are computed and show
  ax1 = nexttile([2,1]);
  hold on;
-  plot(f, abs(tf_est), '-')
+  plot(freqs, abs(squeeze(freqresp(Gi, freqs, 'Hz'))), 'k-')
  plot(freqs, abs(squeeze(freqresp(Gi, freqs, 'Hz'))), '--')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel', []);
  hold off;
@ -1113,14 +1166,12 @@ The transfer function from input voltage to output voltage are computed and show
  ax2 = nexttile;
  hold on;
-  plot(f, 180/pi*angle(tf_est), '-', 'DisplayName', '2 Act - 1 Sen')
+  plot(freqs, 180/pi*angle(squeeze(freqresp(Gi, freqs, 'Hz'))), 'k-')
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gi, freqs, 'Hz'))), '--', 'DisplayName', '2 Act - 1 Sen, - FEM')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
  ylabel('Phase'); xlabel('Frequency [Hz]');
  hold off;
  ylim([-180, 180]);
  yticks(-180:90:180);
  legend('location', 'northeast')
  linkaxes([ax1,ax2], 'x');
  xlim([10, 5e3]);
@ -1131,12 +1182,22 @@ The transfer function from input voltage to output voltage are computed and show
 #+end_src
 #+name: fig:iff_plant_identification_apa95ml
-#+caption: Identification of the IFF plant
+#+caption: Bode plot of the IFF plant
 #+RESULTS:
 [[file:figs/iff_plant_identification_apa95ml.png]]
 The controller used in the Integral Force Feedback Architecture is:
 \begin{equation}
  K_{\text{IFF}}(s) = \frac{g}{s + 2\cdot 2\pi} \cdot \frac{s}{s + 0.5 \cdot 2\pi}
 \end{equation}
 where $g$ is a gain that can be tuned.
 Above 2 Hz the controller is basically an integrator, whereas an high pass filter is added at 0.5Hz to further reduce the low frequency gain.
 In the frequency band of interest, this controller should mostly act as a pure integrator.
 The Root Locus corresponding to this controller is shown in Figure [[fig:root_locus_iff_apa95ml_identification]].
 ** Integral Force Feedback
 #+begin_src matlab :exports none
  gains = logspace(0, 5, 1000);
@ -1164,37 +1225,11 @@ The transfer function from input voltage to output voltage are computed and show
 #+RESULTS:
 [[file:figs/root_locus_iff_apa95ml_identification.png]]
 * Integral Force Feedback
 :PROPERTIES:
 :header-args:matlab+: :tangle matlab/integral_force_feedback.m
 :header-args:matlab+: :comments org :mkdirp yes
 :END:
 <<sec:integral_force_feedback>>
 ** Introduction                                                      :ignore:
 #+name: fig:test_bench_apa_schematic_iff
 #+caption: Schematic of the test bench using IFF
 [[file:figs/test_bench_apa_schematic_iff.png]]
 ** Matlab Init                                             :noexport:ignore:
 #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
 #+end_src
 #+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
 #+end_src
 #+begin_src matlab :tangle no
  addpath('./matlab/mat/');
 #+end_src
 #+begin_src matlab :eval no
  addpath('./mat/');
 #+end_src
 ** First tests with few gains
 The controller is now implemented in practice, and few controller gains are tested: $g = 0$, $g = 10$ and $g = 100$.
 For each controller gain, the identification shown in Figure [[fig:test_bench_apa_schematic_iff]] is performed.
 #+begin_src matlab
  iff_g10  = load('apa95ml_iff_g10_res.mat',  'u', 't', 'y', 'v');
  iff_g100 = load('apa95ml_iff_g100_res.mat', 'u', 't', 'y', 'v');
@ -1215,6 +1250,8 @@ The transfer function from input voltage to output voltage are computed and show
  [co_iff_of, ~] = mscohere(  iff_of.u, iff_of.y, win, [], [], 1/Ts);
 #+end_src
 The coherence between the excitation signal and the mass displacement as measured by the interferometer is shown in Figure [[fig:iff_first_test_coherence]].
 #+begin_src matlab :exports none
  figure;
@ -1238,6 +1275,8 @@ The transfer function from input voltage to output voltage are computed and show
 #+RESULTS:
 [[file:figs/iff_first_test_coherence.png]]
 The obtained transfer functions are shown in Figure [[fig:iff_first_test_bode_plot]].
 It is clear that the IFF architecture can actively damp the main resonance of the system.
 #+begin_src matlab :exports none
  figure;
@ -1277,6 +1316,7 @@ The transfer function from input voltage to output voltage are computed and show
 [[file:figs/iff_first_test_bode_plot.png]]
 ** Second test with many Gains
 Then, the same identification test is performed for many more gains.
 #+begin_src matlab
  load('apa95ml_iff_test.mat', 'results');
 #+end_src
@ -1300,19 +1340,7 @@ The transfer function from input voltage to output voltage are computed and show
  end
 #+end_src
-#+begin_src matlab :exports none
+The obtained dynamics are shown in Figure [[fig:iff_results_bode_plots]].
  figure;
  hold on;
  for i = 1:length(results)
    plot(f, co_iff{i}, '-', 'DisplayName', sprintf('g = %0.f', g_iff(i)))
  end
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
  ylabel('Coherence'); xlabel('Frequency [Hz]');
  hold off;
  legend();
  xlim([60, 600])
 #+end_src
 #+begin_src matlab :exports none
  figure;
@ -1347,10 +1375,12 @@ The transfer function from input voltage to output voltage are computed and show
 #+end_src
 #+name: fig:iff_results_bode_plots
-#+caption:
+#+caption: Identified dynamics from excitation voltage to the mass displacement
 #+RESULTS:
 [[file:figs/iff_results_bode_plots.png]]
 For each gain, the parameters of a second order resonant system that best fits the data are estimated and are compared with the data in Figure [[fig:iff_results_bode_plots_identification]].
 #+begin_src matlab
  G_id = {zeros(1,length(results))};
@ -1406,10 +1436,12 @@ The transfer function from input voltage to output voltage are computed and show
 #+end_src
 #+name: fig:iff_results_bode_plots_identification
-#+caption:
+#+caption: Comparison of the measured dynamic and the identified 2nd order resonant systems that best fits the data
 #+RESULTS:
 [[file:figs/iff_results_bode_plots_identification.png]]
 Finally, we can represent the position of the poles of the 2nd order systems on the Root Locus plot (Figure [[fig:iff_results_root_locus]]).
 #+begin_src matlab :exports none
  w_z = 2*pi*111; % Zeros frequency [rad/s]
  w_p = 2*pi*255; % Pole frequency [rad/s]
@ -1447,6 +1479,6 @@ The transfer function from input voltage to output voltage are computed and show
 #+end_src
 #+name: fig:iff_results_root_locus
-#+caption:
+#+caption: Root Locus plot of the identified IFF plant as well as the identified poles of the damped system
 #+RESULTS:
 [[file:figs/iff_results_root_locus.png]]
--- a/matlab/force_sensor_identification.m
+++ b/matlab/force_sensor_identification.m
@ -0,0 +1,95 @@
 %% Clear Workspace and Close figures
 clear; close all; clc;
 %% Intialize Laplace variable
 s = zpk('s');
 addpath('./mat/');
 % Load Data                                                         :ignore:
 % The data are loaded:
 load('apa95ml_5kg_2a_1s.mat', 't', 'u', 'v');
 % Any offset is removed.
 u = detrend(u, 0); % Speedgoat DAC output Voltage [V]
 v = detrend(v, 0); % Speedgoat ADC input Voltage (sensor stack) [V]
 % Here, the amplifier gain is 20.
 u = 20*u; % Actuator Stack Voltage [V]
 % Compute TF estimate and Coherence                                 :ignore:
 % The transfer function from the actuator voltage $V_a$ to the force sensor stack voltage $V_s$ is computed.
 Ts = t(end)/(length(t)-1);
 Fs = 1/Ts;
 win = hann(ceil(5/Ts));
 [tf_est,  f] = tfestimate(u, v, win, [], [], 1/Ts);
 [coh,     ~] = mscohere(  u, v, win, [], [], 1/Ts);
 % The coherence is shown in Figure [[fig:apa95ml_5kg_cedrat_coh]].
 figure;
 hold on;
 plot(f, coh, 'k-')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
 ylabel('Coherence'); xlabel('Frequency [Hz]');
 hold off;
 xlim([10, 5e3]);
 % #+name: fig:apa95ml_5kg_cedrat_coh
 % #+caption: Coherence
 % #+RESULTS:
 % [[file:figs/apa95ml_5kg_cedrat_coh.png]]
 % The Simscape model is loaded and compared with the identified dynamics in Figure [[fig:bode_plot_force_sensor_voltage_comp_fem]].
 % The non-minimum phase zero is just a side effect of the not so great identification.
 % Taking longer measurements would results in a minimum phase zero.
 load('mat/fem_simscape_models.mat', 'Gfm');
 freqs = logspace(1, 4, 1000);
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 ax1 = nexttile([2,1]);
 hold on;
 plot(f, abs(tf_est))
 plot(freqs, abs(squeeze(freqresp(Gfm, freqs, 'Hz'))))
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
 ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
 hold off;
 ylim([1e-3, 1e1]);
 ax2 = nexttile;
 hold on;
 plot(f, 180/pi*angle(tf_est), ...
     'DisplayName', 'Identification')
 plot(freqs, 180/pi*angle(squeeze(freqresp(Gfm, freqs, 'Hz'))), ...
     'DisplayName', 'FEM')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
 ylabel('Phase'); xlabel('Frequency [Hz]');
 hold off;
 ylim([-180, 180]);
 yticks(-180:90:180);
 legend('location', 'northeast')
 linkaxes([ax1,ax2], 'x');
 xlim([10, 5e3]);
--- a/matlab/huddle_test.m
+++ b/matlab/huddle_test.m
@ -0,0 +1,34 @@
 %% Clear Workspace and Close figures
 clear; close all; clc;
 %% Intialize Laplace variable
 s = zpk('s');
 addpath('./mat/');
 % Load Data                                                       :noexport:
 load('huddle_test.mat', 't', 'y');
 y = y - mean(y(1000:end));
 % Time Domain Data
 figure;
 plot(t(1000:end), y(1000:end))
 ylabel('Output Displacement [m]'); xlabel('Time [s]');
 % PSD of Measurement Noise
 Ts = t(end)/(length(t)-1);
 Fs = 1/Ts;
 win = hanning(ceil(1*Fs));
 [pxx, f] = pwelch(y(1000:end), win, [], [], Fs);
 figure;
 plot(f, sqrt(pxx));
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
 xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
 xlim([1, Fs/2]);
--- a/matlab/integral_force_feedback.m
+++ b/matlab/integral_force_feedback.m
@ -0,0 +1,307 @@
 %% Clear Workspace and Close figures
 clear; close all; clc;
 %% Intialize Laplace variable
 s = zpk('s');
 addpath('./mat/');
 % IFF Plant
 % From the identified plant, a model of the transfer function from the actuator stack voltage to the force sensor generated voltage is developed.
 w_z = 2*pi*111; % Zeros frequency [rad/s]
 w_p = 2*pi*255; % Pole frequency [rad/s]
 xi_z = 0.05;
 xi_p = 0.015;
 G_inf = 0.1;
 Gi = G_inf*(s^2 + 2*xi_z*w_z*s + w_z^2)/(s^2 + 2*xi_p*w_p*s + w_p^2);
 % Its bode plot is shown in Figure [[fig:iff_plant_identification_apa95ml]].
 freqs = logspace(1, 4, 1000);
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 ax1 = nexttile([2,1]);
 hold on;
 plot(freqs, abs(squeeze(freqresp(Gi, freqs, 'Hz'))), 'k-')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
 ylabel('Amplitude'); set(gca, 'XTickLabel', []);
 hold off;
 ylim([1e-3, 1e1]);
 ax2 = nexttile;
 hold on;
 plot(freqs, 180/pi*angle(squeeze(freqresp(Gi, freqs, 'Hz'))), 'k-')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
 ylabel('Phase'); xlabel('Frequency [Hz]');
 hold off;
 ylim([-180, 180]);
 yticks(-180:90:180);
 linkaxes([ax1,ax2], 'x');
 xlim([10, 5e3]);
 % #+name: fig:iff_plant_identification_apa95ml
 % #+caption: Bode plot of the IFF plant
 % #+RESULTS:
 % [[file:figs/iff_plant_identification_apa95ml.png]]
 % The controller used in the Integral Force Feedback Architecture is:
 % \begin{equation}
 %   K_{\text{IFF}}(s) = \frac{g}{s + 2\cdot 2\pi} \cdot \frac{s}{s + 0.5 \cdot 2\pi}
 % \end{equation}
 % where $g$ is a gain that can be tuned.
 % Above 2 Hz the controller is basically an integrator, whereas an high pass filter is added at 0.5Hz to further reduce the low frequency gain.
 % In the frequency band of interest, this controller should mostly act as a pure integrator.
 % The Root Locus corresponding to this controller is shown in Figure [[fig:root_locus_iff_apa95ml_identification]].
 gains = logspace(0, 5, 1000);
 figure;
 hold on;
 plot(real(pole(Gi)),  imag(pole(Gi)),  'kx');
 plot(real(tzero(Gi)),  imag(tzero(Gi)),  'ko');
 for i = 1:length(gains)
  cl_poles = pole(feedback(Gi, (gains(i)/(s + 2*2*pi)*s/(s + 0.5*2*pi))));
  plot(real(cl_poles), imag(cl_poles), 'k.');
 end
 ylim([0, 1800]);
 xlim([-1600,200]);
 xlabel('Real Part')
 ylabel('Imaginary Part')
 axis square
 % First tests with few gains
 % The controller is now implemented in practice, and few controller gains are tested: $g = 0$, $g = 10$ and $g = 100$.
 % For each controller gain, the identification shown in Figure [[fig:test_bench_apa_schematic_iff]] is performed.
 iff_g10  = load('apa95ml_iff_g10_res.mat',  'u', 't', 'y', 'v');
 iff_g100 = load('apa95ml_iff_g100_res.mat', 'u', 't', 'y', 'v');
 iff_of   = load('apa95ml_iff_off_res.mat',  'u', 't', 'y', 'v');
 Ts = 1e-4;
 win = hann(ceil(10/Ts));
 [tf_iff_g10, f] = tfestimate(iff_g10.u, iff_g10.y, win, [], [], 1/Ts);
 [co_iff_g10, ~] = mscohere(  iff_g10.u, iff_g10.y, win, [], [], 1/Ts);
 [tf_iff_g100, ~] = tfestimate(iff_g100.u, iff_g100.y, win, [], [], 1/Ts);
 [co_iff_g100, ~] = mscohere(  iff_g100.u, iff_g100.y, win, [], [], 1/Ts);
 [tf_iff_of, ~] = tfestimate(iff_of.u, iff_of.y, win, [], [], 1/Ts);
 [co_iff_of, ~] = mscohere(  iff_of.u, iff_of.y, win, [], [], 1/Ts);
 % The coherence between the excitation signal and the mass displacement as measured by the interferometer is shown in Figure [[fig:iff_first_test_coherence]].
 figure;
 hold on;
 plot(f, co_iff_of, '-', 'DisplayName', 'g=0')
 plot(f, co_iff_g10, '-', 'DisplayName', 'g=10')
 plot(f, co_iff_g100, '-', 'DisplayName', 'g=100')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
 ylabel('Coherence'); xlabel('Frequency [Hz]');
 hold off;
 legend();
 xlim([60, 600])
 % #+name: fig:iff_first_test_coherence
 % #+caption: Coherence
 % #+RESULTS:
 % [[file:figs/iff_first_test_coherence.png]]
 % The obtained transfer functions are shown in Figure [[fig:iff_first_test_bode_plot]].
 % It is clear that the IFF architecture can actively damp the main resonance of the system.
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 ax1 = nexttile([2, 1]);
 hold on;
 plot(f, abs(tf_iff_of), '-', 'DisplayName', 'g=0')
 plot(f, abs(tf_iff_g10), '-', 'DisplayName', 'g=10')
 plot(f, abs(tf_iff_g100), '-', 'DisplayName', 'g=100')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
 ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
 hold off;
 legend();
 ax2 = nexttile;
 hold on;
 plot(f, 180/pi*angle(-tf_iff_of), '-')
 plot(f, 180/pi*angle(-tf_iff_g10), '-')
 plot(f, 180/pi*angle(-tf_iff_g100), '-')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
 ylabel('Phase'); xlabel('Frequency [Hz]');
 hold off;
 yticks(-360:90:360);
 linkaxes([ax1,ax2], 'x');
 xlim([60, 600]);
 % Second test with many Gains
 % Then, the same identification test is performed for many more gains.
 load('apa95ml_iff_test.mat', 'results');
 Ts = 1e-4;
 win = hann(ceil(10/Ts));
 tf_iff = {zeros(1, length(results))};
 co_iff = {zeros(1, length(results))};
 g_iff = [0, 1, 5, 10, 50, 100];
 for i=1:length(results)
    [tf_est, f] = tfestimate(results{i}.u, results{i}.y, win, [], [], 1/Ts);
    [co_est, ~] = mscohere(results{i}.u, results{i}.y, win, [], [], 1/Ts);
    tf_iff(i) = {tf_est};
    co_iff(i) = {co_est};
 end
 % The obtained dynamics are shown in Figure [[fig:iff_results_bode_plots]].
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 ax1 = nexttile([2, 1]);
 hold on;
 for i = 1:length(results)
  plot(f, abs(tf_iff{i}), '-', 'DisplayName', sprintf('g = %0.f', g_iff(i)))
 end
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
 ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
 hold off;
 legend();
 ax2 = nexttile;
 hold on;
 for i = 1:length(results)
  plot(f, 180/pi*angle(-tf_iff{i}), '-')
 end
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
 ylabel('Phase'); xlabel('Frequency [Hz]');
 yticks(-360:90:360);
 axis padded 'auto x'
 linkaxes([ax1,ax2], 'x');
 xlim([60, 600]);
 % #+name: fig:iff_results_bode_plots
 % #+caption: Identified dynamics from excitation voltage to the mass displacement
 % #+RESULTS:
 % [[file:figs/iff_results_bode_plots.png]]
 % For each gain, the parameters of a second order resonant system that best fits the data are estimated and are compared with the data in Figure [[fig:iff_results_bode_plots_identification]].
 G_id = {zeros(1,length(results))};
 f_start = 70; % [Hz]
 f_end = 500; % [Hz]
 for i = 1:length(results)
    tf_id = tf_iff{i}(sum(f<f_start):length(f)-sum(f>f_end));
    f_id = f(sum(f<f_start):length(f)-sum(f>f_end));
    gfr = idfrd(tf_id, 2*pi*f_id, Ts);
    G_id(i) = {procest(gfr,'P2UDZ')};
 end
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 ax1 = nexttile([2, 1]);
 hold on;
 for i = 1:length(results)
  set(gca,'ColorOrderIndex',i)
  plot(f, abs(tf_iff{i}), '-', 'DisplayName', sprintf('g = %0.f', g_iff(i)))
  set(gca,'ColorOrderIndex',i)
  plot(f, abs(squeeze(freqresp(G_id{i}, f, 'Hz'))), '--', 'HandleVisibility', 'off')
 end
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
 ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
 hold off;
 legend();
 ax2 = nexttile;
 hold on;
 for i = 1:length(results)
  set(gca,'ColorOrderIndex',i)
  plot(f, 180/pi*angle(tf_iff{i}), '-')
  set(gca,'ColorOrderIndex',i)
  plot(f, 180/pi*angle(squeeze(freqresp(G_id{i}, f, 'Hz'))), '--')
 end
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
 ylabel('Phase'); xlabel('Frequency [Hz]');
 hold off;
 yticks(-360:90:360);
 axis padded 'auto x'
 linkaxes([ax1,ax2], 'x');
 xlim([60, 600]);
 % #+name: fig:iff_results_bode_plots_identification
 % #+caption: Comparison of the measured dynamic and the identified 2nd order resonant systems that best fits the data
 % #+RESULTS:
 % [[file:figs/iff_results_bode_plots_identification.png]]
 % Finally, we can represent the position of the poles of the 2nd order systems on the Root Locus plot (Figure [[fig:iff_results_root_locus]]).
 w_z = 2*pi*111; % Zeros frequency [rad/s]
 w_p = 2*pi*255; % Pole frequency [rad/s]
 xi_z = 0.05;
 xi_p = 0.015;
 G_inf = 2;
 Gi = G_inf*(s^2 - 2*xi_z*w_z*s + w_z^2)/(s^2 + 2*xi_p*w_p*s + w_p^2);
 gains = logspace(0, 5, 1000);
 figure;
 hold on;
 plot(real(pole(Gi)),  imag(pole(Gi)),  'kx', 'HandleVisibility', 'off');
 plot(real(tzero(Gi)),  imag(tzero(Gi)),  'ko', 'HandleVisibility', 'off');
 for i = 1:length(results)
  set(gca,'ColorOrderIndex',i)
  plot(real(pole(G_id{i})), imag(pole(G_id{i})), 'o', 'DisplayName', sprintf('g = %0.f', g_iff(i)));
 end
 for i = 1:length(gains)
  cl_poles = pole(feedback(Gi, (gains(i)/(s + 2*pi*2))));
  plot(real(cl_poles), imag(cl_poles), 'k.', 'HandleVisibility', 'off');
 end
 ylim([0, 1800]);
 xlim([-1600,200]);
 xlabel('Real Part')
 ylabel('Imaginary Part')
 axis square
 legend('location', 'northwest');
--- a/matlab/mat/fem_simscape_models.mat
+++ b/matlab/mat/fem_simscape_models.mat
--- a/matlab/motion_identification.m
+++ b/matlab/motion_identification.m
@ -0,0 +1,100 @@
 %% Clear Workspace and Close figures
 clear; close all; clc;
 %% Intialize Laplace variable
 s = zpk('s');
 addpath('./mat/');
 % Load Data
 % The data from the "noise test" and the identification test are loaded.
 ht = load('huddle_test.mat', 't', 'y');
 load('apa95ml_5kg_Amp_E505.mat', 't', 'um', 'y');
 % Any offset value is removed:
 um = detrend(um, 0); % Generated DAC Voltage [V]
 y  = detrend(y , 0); % Mass displacement [m]
 ht.y  = detrend(ht.y, 0);
 % Now we add a factor 10 to take into account the gain of the voltage amplifier.
 Va = 10*um;
 % Comparison of the PSD with Huddle Test
 Ts = t(end)/(length(t)-1);
 Fs = 1/Ts;
 win = hanning(ceil(1*Fs));
 [pxx, f] = pwelch(y, win, [], [], Fs);
 [pht, ~] = pwelch(ht.y, win, [], [], Fs);
 figure;
 hold on;
 plot(f, sqrt(pxx), 'DisplayName', '5kg');
 plot(f, sqrt(pht), 'DisplayName', 'Huddle Test');
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
 xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
 legend('location', 'northeast');
 xlim([1, Fs/2]);
 % Compute TF estimate and Coherence
 [tf_est, f] = tfestimate(Va, -y, win, [], [], 1/Ts);
 [co_est, ~] = mscohere(  Va, -y, win, [], [], 1/Ts);
 figure;
 hold on;
 plot(f, co_est, 'k-')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
 ylabel('Coherence'); xlabel('Frequency [Hz]');
 hold off;
 xlim([10, 5e3]);
 % #+name: fig:apa95ml_5kg_PI_coh
 % #+caption: Coherence
 % #+RESULTS:
 % [[file:figs/apa95ml_5kg_PI_coh.png]]
 % Comparison with the FEM model
 load('mat/fem_simscape_models.mat', 'Ghm');
 freqs = logspace(0, 4, 1000);
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 ax1 = nexttile([2,1]);
 hold on;
 plot(f, abs(tf_est), 'DisplayName', 'Identification')
 plot(freqs, abs(squeeze(freqresp(Ghm, freqs, 'Hz'))), 'DisplayName', 'FEM')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
 ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel', []);
 legend('location', 'northeast')
 hold off;
 ax2 = nexttile;
 hold on;
 plot(f,     180/pi*angle(tf_est))
 plot(freqs, 180/pi*angle(squeeze(freqresp(Ghm, freqs, 'Hz'))))
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
 ylabel('Phase'); xlabel('Frequency [Hz]');
 hold off;
 ylim([-180, 180]);
 yticks(-180:90:180);
 linkaxes([ax1,ax2], 'x');
 xlim([10, 5e3]);
--- a/matlab/piezo_amplified_3d.slx
+++ b/matlab/piezo_amplified_3d.slx
--- a/matlab/piezo_parameters.m
+++ b/matlab/piezo_parameters.m
@ -38,7 +38,7 @@ ka = 235e6; % [N/m]
 Lmax = 20e-6; % [m]
 Vmax = 170; % [V]
-ka*Lmax/Vmax % [N/V]
+ga = ka*Lmax/Vmax; % [N/V]
 % Estimation from Piezoelectric parameters
 % <<sec:estimation_piezo_params>>
--- a/matlab/simscape_model.m
+++ b/matlab/simscape_model.m
@ -0,0 +1,120 @@
 %% Clear Workspace and Close figures
 clear; close all; clc;
 %% Intialize Laplace variable
 s = zpk('s');
 addpath('./mat/');
 % Import Mass Matrix, Stiffness Matrix, and Interface Nodes Coordinates
 % We first extract the stiffness and mass matrices.
 K = readmatrix('APA95ML_K.CSV');
 M = readmatrix('APA95ML_M.CSV');
 % #+caption: First 10x10 elements of the Mass matrix
 % #+RESULTS:
 % |    0.03 |   7e-08 |  2e-06 | -3e-09 | -0.0002 | -6e-08 | -0.001 |   8e-07 |  6e-07 | -8e-09 |
 % |   7e-08 |    0.02 | -1e-06 |  9e-05 |  -3e-09 | -4e-09 | -1e-06 | -0.0006 | -4e-08 |  5e-06 |
 % |   2e-06 |  -1e-06 |   0.02 | -3e-08 |  -4e-08 |  1e-08 |  1e-07 |  -2e-07 | 0.0003 |  1e-09 |
 % |  -3e-09 |   9e-05 | -3e-08 |  1e-06 |  -3e-11 | -3e-13 | -7e-09 |  -5e-06 | -3e-10 |  3e-08 |
 % | -0.0002 |  -3e-09 | -4e-08 | -3e-11 |   2e-06 |  6e-10 |  2e-06 |  -7e-09 | -2e-09 |  7e-11 |
 % |  -6e-08 |  -4e-09 |  1e-08 | -3e-13 |   6e-10 |  1e-06 |  1e-08 |   3e-09 | -2e-09 |  2e-13 |
 % |  -0.001 |  -1e-06 |  1e-07 | -7e-09 |   2e-06 |  1e-08 |   0.03 |   4e-08 | -2e-06 |  8e-09 |
 % |   8e-07 | -0.0006 | -2e-07 | -5e-06 |  -7e-09 |  3e-09 |  4e-08 |    0.02 | -9e-07 | -9e-05 |
 % |   6e-07 |  -4e-08 | 0.0003 | -3e-10 |  -2e-09 | -2e-09 | -2e-06 |  -9e-07 |   0.02 |  2e-08 |
 % |  -8e-09 |   5e-06 |  1e-09 |  3e-08 |   7e-11 |  2e-13 |  8e-09 |  -9e-05 |  2e-08 |  1e-06 |
 % Then, we extract the coordinates of the interface nodes.
 [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA95ML_out_nodes_3D.txt');
 % Simscape Model
 % The flexible element is imported using the =Reduced Order Flexible Solid= Simscape block.
 % To model the actuator, an =Internal Force= block is added between the nodes 3 and 12.
 % A =Relative Motion Sensor= block is added between the nodes 1 and 2 to measure the displacement and the amplified piezo.
 % One mass is fixed at one end of the piezo-electric stack actuator, the other end is fixed to the world frame.
 m = 5.5;
 load('apa95ml_params.mat', 'ga', 'gs');
 % Dynamics from Actuator Voltage to Vertical Mass Displacement
 % The identified dynamics is shown in Figure [[fig:dynamics_act_disp_comp_mass]].
 %% Name of the Simulink File
 mdl = 'piezo_amplified_3d';
 %% Input/Output definition
 clear io; io_i = 1;
 io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1; % Actuator Voltage [V]
 io(io_i) = linio([mdl, '/y'],  1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
 Ghm = linearize(mdl, io);
 freqs = logspace(0, 4, 5000);
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 ax1 = nexttile([2,1]);
 hold on;
 plot(freqs, abs(squeeze(freqresp(Ghm, freqs, 'Hz'))));
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
 ylabel('Amplitude $d/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
 hold off;
 ax2 = nexttile;
 hold on;
 plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Ghm, freqs, 'Hz')))));
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
 yticks(-360:90:360);
 xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
 hold off;
 linkaxes([ax1,ax2],'x');
 xlim([freqs(1), freqs(end)]);
 % Dynamics from Actuator Voltage to Force Sensor Voltage
 % The obtained dynamics is shown in Figure [[fig:dynamics_force_force_sensor_comp_mass]].
 %% Name of the Simulink File
 mdl = 'piezo_amplified_3d';
 %% Input/Output definition
 clear io; io_i = 1;
 io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1; % Voltage Actuator [V]
 io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage [V]
 Gfm = linearize(mdl, io);
 freqs = logspace(1, 5, 1000);
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 ax1 = nexttile([2,1]);
 hold on;
 plot(freqs, abs(squeeze(freqresp(Gfm, freqs, 'Hz'))));
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
 ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
 hold off;
 ax2 = nexttile;
 hold on;
 plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gfm, freqs, 'Hz')))));
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
 yticks(-360:90:360); ylim([-360, 180]);
 xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
 hold off;
 linkaxes([ax1,ax2],'x');
 xlim([freqs(1), freqs(end)]);
 save('mat/fem_simscape_models.mat', 'Ghm', 'Gfm')