diff --git a/active_damping/index.html b/active_damping/index.html
index 32bd328..d4e19e6 100644
--- a/active_damping/index.html
+++ b/active_damping/index.html
@@ -1,12 +1,13 @@
 <?xml version="1.0" encoding="utf-8"?>
+<?xml version="1.0" encoding="utf-8"?>
 <!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
 "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
 <html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
 <head>
-<!-- 2019-12-11 mer. 16:55 -->
+<!-- 2020-01-15 mer. 16:22 -->
 <meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
 <meta name="viewport" content="width=device-width, initial-scale=1" />
-<title>Active Damping</title>
+<title>Active Damping applied on the Simscape Model</title>
 <meta name="generator" content="Org mode" />
 <meta name="author" content="Dehaeze Thomas" />
 <style type="text/css">
@@ -205,7 +206,7 @@
 @licstart  The following is the entire license notice for the
 JavaScript code in this tag.
 
-Copyright (C) 2012-2019 Free Software Foundation, Inc.
+Copyright (C) 2012-2020 Free Software Foundation, Inc.
 
 The JavaScript code in this tag is free software: you can
 redistribute it and/or modify it under the terms of the GNU
@@ -278,95 +279,132 @@ for the JavaScript code in this tag.
  |
  <a accesskey="H" href="../index.html"> HOME </a>
 </div><div id="content">
-<h1 class="title">Active Damping</h1>
+<h1 class="title">Active Damping applied on the Simscape Model</h1>
 <div id="table-of-contents">
 <h2>Table of Contents</h2>
 <div id="text-table-of-contents">
 <ul>
-<li><a href="#org0c00f50">1. Undamped System</a>
+<li><a href="#org43bc922">1. Undamped System</a>
 <ul>
-<li><a href="#orgf1476fa">1.1. Init</a></li>
-<li><a href="#orgd3d2ff0">1.2. Identification</a></li>
-<li><a href="#orge1d51ce">1.3. Sensitivity to disturbances</a></li>
-<li><a href="#org58ffec6">1.4. Undamped Plant</a></li>
+<li><a href="#org9a8111b">1.1. Identification of the dynamics for Active Damping</a>
+<ul>
+<li><a href="#org813e9d0">1.1.1. Initialize the Simulation</a></li>
+<li><a href="#org85a96c0">1.1.2. Identification</a></li>
+<li><a href="#orgc478ad1">1.1.3. Obtained Plants for Active Damping</a></li>
 </ul>
 </li>
-<li><a href="#orgf769f9d">2. Integral Force Feedback</a>
+<li><a href="#org22150e6">1.2. Tomography Experiment</a>
 <ul>
-<li><a href="#orgf0e86b6">2.1. One degree-of-freedom example</a>
-<ul>
-<li><a href="#org00cf95e">2.1.1. Equations</a></li>
-<li><a href="#org227d2a7">2.1.2. Matlab Example</a></li>
+<li><a href="#org931bbc3">1.2.1. Simulation</a></li>
+<li><a href="#org97e15e5">1.2.2. Results</a></li>
 </ul>
 </li>
-<li><a href="#orga9d4adb">2.2. Control Design</a></li>
-<li><a href="#org582d26c">2.3. Identification of the damped plant</a></li>
-<li><a href="#org231207b">2.4. Sensitivity to disturbances</a></li>
-<li><a href="#org9157c29">2.5. Damped Plant</a></li>
-<li><a href="#org79a4305">2.6. Conclusion</a></li>
 </ul>
 </li>
-<li><a href="#org56506d8">3. Relative Motion Control</a>
+<li><a href="#orgeca85c7">2. Integral Force Feedback</a>
 <ul>
-<li><a href="#org6502b14">3.1. One degree-of-freedom example</a>
+<li><a href="#org8e730eb">2.1. Control Design</a>
 <ul>
-<li><a href="#orga4b0d18">3.1.1. Equations</a></li>
-<li><a href="#org6043cef">3.1.2. Matlab Example</a></li>
+<li><a href="#org4aae4a5">2.1.1. Plant</a></li>
+<li><a href="#org9230021">2.1.2. Control Design</a></li>
+<li><a href="#orgc48be49">2.1.3. Diagonal Controller</a></li>
 </ul>
 </li>
-<li><a href="#orgdf908e5">3.2. Control Design</a></li>
-<li><a href="#org9792b77">3.3. Identification of the damped plant</a></li>
-<li><a href="#org4894be4">3.4. Sensitivity to disturbances</a></li>
-<li><a href="#orgd89441e">3.5. Damped Plant</a></li>
-<li><a href="#org04f7d21">3.6. Conclusion</a></li>
-</ul>
-</li>
-<li><a href="#org9af791d">4. Direct Velocity Feedback</a>
+<li><a href="#org521b83c">2.2. Tomography Experiment</a>
 <ul>
-<li><a href="#orgda71fbe">4.1. One degree-of-freedom example</a>
+<li><a href="#org80693d0">2.2.1. Initialize the Simulation</a></li>
+<li><a href="#org828fe0c">2.2.2. Simulation</a></li>
+<li><a href="#orgba367cd">2.2.3. Compare with Undamped system</a></li>
+</ul>
+</li>
+<li><a href="#org64c89c0">2.3. Conclusion</a></li>
+</ul>
+</li>
+<li><a href="#org1bec1c8">3. Direct Velocity Feedback</a>
 <ul>
-<li><a href="#org332c25e">4.1.1. Equations</a></li>
-<li><a href="#org94d4a25">4.1.2. Matlab Example</a></li>
-</ul>
-</li>
-<li><a href="#org78afe53">4.2. Control Design</a></li>
-<li><a href="#orgb4d1b92">4.3. Identification of the damped plant</a></li>
-<li><a href="#org01d1d1c">4.4. Sensitivity to disturbances</a></li>
-<li><a href="#orgeb00466">4.5. Damped Plant</a></li>
-<li><a href="#orgfb13592">4.6. Conclusion</a></li>
-</ul>
-</li>
-<li><a href="#org8970afd">5. Comparison</a>
+<li><a href="#org34fcfd6">3.1. Control Design</a>
 <ul>
-<li><a href="#orgb3b1324">5.1. Load the plants</a></li>
-<li><a href="#orga2db271">5.2. Sensitivity to Disturbance</a></li>
-<li><a href="#org0d6cdc3">5.3. Damped Plant</a></li>
+<li><a href="#org2e5695f">3.1.1. Plant</a></li>
+<li><a href="#org8feed7c">3.1.2. Control Design</a></li>
+<li><a href="#org5d88841">3.1.3. Diagonal Controller</a></li>
+</ul>
+</li>
+<li><a href="#orgd0cf173">3.2. Tomography Experiment</a>
+<ul>
+<li><a href="#org41f51f2">3.2.1. Initialize the Simulation</a></li>
+<li><a href="#org0da77b8">3.2.2. Simulation</a></li>
+<li><a href="#org8956847">3.2.3. Compare with Undamped system</a></li>
+</ul>
+</li>
+<li><a href="#org4a46d6a">3.3. Conclusion</a></li>
+</ul>
+</li>
+<li><a href="#orgf8012d0">4. Inertial Control</a>
+<ul>
+<li><a href="#org47e2207">4.1. Control Design</a>
+<ul>
+<li><a href="#orga1c06d5">4.1.1. Plant</a></li>
+<li><a href="#orgee9c27b">4.1.2. Control Design</a></li>
+<li><a href="#org6241541">4.1.3. Diagonal Controller</a></li>
+</ul>
+</li>
+<li><a href="#org9625401">4.2. Tomography Experiment</a>
+<ul>
+<li><a href="#orgf3c8835">4.2.1. Initialize the Simulation</a></li>
+<li><a href="#org457a68a">4.2.2. Simulation</a></li>
+<li><a href="#org5c8213d">4.2.3. Compare with Undamped system</a></li>
+</ul>
+</li>
+<li><a href="#org4e39ecb">4.3. Conclusion</a></li>
+</ul>
+</li>
+<li><a href="#orgb97eda0">5. Comparison</a>
+<ul>
+<li><a href="#org7e7c35d">5.1. Load the plants</a></li>
+<li><a href="#org7c0e10c">5.2. Sensitivity to Disturbance</a></li>
+<li><a href="#org34d3217">5.3. Damped Plant</a></li>
+<li><a href="#orgc2cfa6b">5.4. Tomography Experiment</a>
+<ul>
+<li><a href="#orgb357d35">5.4.1. Frequency Domain</a></li>
+</ul>
+</li>
+</ul>
+</li>
+<li><a href="#orgb95a1fb">6. Useful Functions</a>
+<ul>
+<li><a href="#org624fc0d">6.1. prepareTomographyExperiment</a>
+<ul>
+<li><a href="#org4917bc7">6.1.1. Function Description</a></li>
+<li><a href="#org3c7b365">6.1.2. Optional Parameters</a></li>
+<li><a href="#org758e52a">6.1.3. Initialize the Simulation</a></li>
+</ul>
+</li>
 </ul>
 </li>
-<li><a href="#org0c54368">6. Conclusion</a></li>
 </ul>
 </div>
 </div>
 
 <p>
-First, in section <a href="#orgebcec60">1</a>, we will looked at the undamped system.
+First, in section <a href="#orgcd6de97">1</a>, we will looked at the undamped system.
 </p>
 
 <p>
 Then, we will compare three active damping techniques:
 </p>
 <ul class="org-ul">
-<li>In section <a href="#org6c8af88">2</a>: the integral force feedback is used</li>
-<li>In section <a href="#org5aaa6c2">3</a>: the relative motion control is used</li>
-<li>In section <a href="#org6b8a067">4</a>: the direct velocity feedback is used</li>
+<li>In section <a href="#org45880cb">2</a>: the integral force feedback is used</li>
+<li>In section <a href="#org88df20b">3</a>: the direct velocity feedback is used</li>
+<li>In section <a href="#orgcb7853a">4</a>: inertial control is used</li>
 </ul>
 
 <p>
 For each of the active damping technique, we will:
 </p>
 <ul class="org-ul">
-<li>Compare the sensitivity from disturbances</li>
 <li>Look at the damped plant</li>
+<li>Simulate tomography experiments</li>
+<li>Compare the sensitivity from disturbances</li>
 </ul>
 
 <p>
@@ -374,15 +412,14 @@ The disturbances are:
 </p>
 <ul class="org-ul">
 <li>Ground motion</li>
-<li>Direct forces</li>
 <li>Motion errors of all the stages</li>
 </ul>
 
-<div id="outline-container-org0c00f50" class="outline-2">
-<h2 id="org0c00f50"><span class="section-number-2">1</span> Undamped System</h2>
+<div id="outline-container-org43bc922" class="outline-2">
+<h2 id="org43bc922"><span class="section-number-2">1</span> Undamped System</h2>
 <div class="outline-text-2" id="text-1">
 <p>
-<a id="orgebcec60"></a>
+<a id="orgcd6de97"></a>
 </p>
 <div class="note">
 <p>
@@ -396,112 +433,216 @@ The performance of this undamped system will be compared with the damped system
 </p>
 </div>
 
-<div id="outline-container-orgf1476fa" class="outline-3">
-<h3 id="orgf1476fa"><span class="section-number-3">1.1</span> Init</h3>
+<div id="outline-container-org9a8111b" class="outline-3">
+<h3 id="org9a8111b"><span class="section-number-3">1.1</span> Identification of the dynamics for Active Damping</h3>
 <div class="outline-text-3" id="text-1-1">
+</div>
+<div id="outline-container-org813e9d0" class="outline-4">
+<h4 id="org813e9d0"><span class="section-number-4">1.1.1</span> Initialize the Simulation</h4>
+<div class="outline-text-4" id="text-1-1-1">
 <p>
 We initialize all the stages with the default parameters.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+initializeAxisc();
+initializeMirror();
+</pre>
+</div>
+
+<p>
 The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
 </p>
-
 <div class="org-src-container">
-<pre class="src src-matlab">initializeReferences<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeGround<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeGranite<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeTy<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeRy<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeRz<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeMicroHexapod<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeAxisc<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeMirror<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeNanoHexapod<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'actuator'</span>, <span class="org-string">'piezo'</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-initializeSample<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'mass'</span>, <span class="org-highlight-numbers-number">50</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
+<pre class="src src-matlab">initializeNanoHexapod(<span class="org-string">'actuator'</span>, <span class="org-string">'piezo'</span>);
+initializeSample(<span class="org-string">'mass'</span>, 50);
 </pre>
 </div>
 
 <p>
-All the controllers are set to 0.
+We set the references to zero.
 </p>
 <div class="org-src-container">
-<pre class="src src-matlab">K = tf<span class="org-rainbow-delimiters-depth-1">(</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-K_iff = tf<span class="org-rainbow-delimiters-depth-1">(</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_iff'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-K_rmc = tf<span class="org-rainbow-delimiters-depth-1">(</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_rmc'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-K_dvf = tf<span class="org-rainbow-delimiters-depth-1">(</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_dvf'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
+<pre class="src src-matlab">initializeReferences();
+</pre>
+</div>
+
+<p>
+And all the controllers are set to 0.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">K = tf(zeros(6));
+save(<span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K'</span>, <span class="org-string">'-append'</span>);
+K_ine = tf(zeros(6));
+save(<span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_ine'</span>, <span class="org-string">'-append'</span>);
+K_iff = tf(zeros(6));
+save(<span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_iff'</span>, <span class="org-string">'-append'</span>);
+K_dvf = tf(zeros(6));
+save(<span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_dvf'</span>, <span class="org-string">'-append'</span>);
 </pre>
 </div>
 </div>
 </div>
 
-<div id="outline-container-orgd3d2ff0" class="outline-3">
-<h3 id="orgd3d2ff0"><span class="section-number-3">1.2</span> Identification</h3>
+<div id="outline-container-org85a96c0" class="outline-4">
+<h4 id="org85a96c0"><span class="section-number-4">1.1.2</span> Identification</h4>
+<div class="outline-text-4" id="text-1-1-2">
+<p>
+First, we identify the dynamics of the system using the <code>linearize</code> function.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Options for Linearized</span></span>
+options = linearizeOptions;
+options.SampleTime = 0;
+
+<span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
+mdl = <span class="org-string">'sim_nass_active_damping'</span>;
+
+<span class="org-matlab-cellbreak"><span class="org-comment">%% Input/Output definition</span></span>
+clear io; io_i = 1;
+io(io_i) = linio([mdl, <span class="org-string">'/Fnl'</span>],           1, <span class="org-string">'openinput'</span>);              io_i = io_i <span class="org-type">+</span> 1;
+io(io_i) = linio([mdl, <span class="org-string">'/Micro-Station'</span>], 3, <span class="org-string">'openoutput'</span>, [], <span class="org-string">'Dnlm'</span>); io_i = io_i <span class="org-type">+</span> 1;
+io(io_i) = linio([mdl, <span class="org-string">'/Micro-Station'</span>], 3, <span class="org-string">'openoutput'</span>, [], <span class="org-string">'Fnlm'</span>); io_i = io_i <span class="org-type">+</span> 1;
+io(io_i) = linio([mdl, <span class="org-string">'/Micro-Station'</span>], 3, <span class="org-string">'openoutput'</span>, [], <span class="org-string">'Vlm'</span>);  io_i = io_i <span class="org-type">+</span> 1;
+
+<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
+G = linearize(mdl, io, options);
+G.InputName  = {<span class="org-string">'Fnl1'</span>, <span class="org-string">'Fnl2'</span>, <span class="org-string">'Fnl3'</span>, <span class="org-string">'Fnl4'</span>, <span class="org-string">'Fnl5'</span>, <span class="org-string">'Fnl6'</span>};
+G.OutputName = {<span class="org-string">'Dnlm1'</span>, <span class="org-string">'Dnlm2'</span>, <span class="org-string">'Dnlm3'</span>, <span class="org-string">'Dnlm4'</span>, <span class="org-string">'Dnlm5'</span>, <span class="org-string">'Dnlm6'</span>, ...
+                <span class="org-string">'Fnlm1'</span>, <span class="org-string">'Fnlm2'</span>, <span class="org-string">'Fnlm3'</span>, <span class="org-string">'Fnlm4'</span>, <span class="org-string">'Fnlm5'</span>, <span class="org-string">'Fnlm6'</span>, ...
+                <span class="org-string">'Vnlm1'</span>, <span class="org-string">'Vnlm2'</span>, <span class="org-string">'Vnlm3'</span>, <span class="org-string">'Vnlm4'</span>, <span class="org-string">'Vnlm5'</span>, <span class="org-string">'Vnlm6'</span>};
+</pre>
+</div>
+
+<p>
+We then create transfer functions corresponding to the active damping plants.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">G_iff = minreal(G({<span class="org-string">'Fnlm1'</span>, <span class="org-string">'Fnlm2'</span>, <span class="org-string">'Fnlm3'</span>, <span class="org-string">'Fnlm4'</span>, <span class="org-string">'Fnlm5'</span>, <span class="org-string">'Fnlm6'</span>}, {<span class="org-string">'Fnl1'</span>, <span class="org-string">'Fnl2'</span>, <span class="org-string">'Fnl3'</span>, <span class="org-string">'Fnl4'</span>, <span class="org-string">'Fnl5'</span>, <span class="org-string">'Fnl6'</span>}));
+G_dvf = minreal(G({<span class="org-string">'Dnlm1'</span>, <span class="org-string">'Dnlm2'</span>, <span class="org-string">'Dnlm3'</span>, <span class="org-string">'Dnlm4'</span>, <span class="org-string">'Dnlm5'</span>, <span class="org-string">'Dnlm6'</span>}, {<span class="org-string">'Fnl1'</span>, <span class="org-string">'Fnl2'</span>, <span class="org-string">'Fnl3'</span>, <span class="org-string">'Fnl4'</span>, <span class="org-string">'Fnl5'</span>, <span class="org-string">'Fnl6'</span>}));
+G_ine = minreal(G({<span class="org-string">'Vnlm1'</span>, <span class="org-string">'Vnlm2'</span>, <span class="org-string">'Vnlm3'</span>, <span class="org-string">'Vnlm4'</span>, <span class="org-string">'Vnlm5'</span>, <span class="org-string">'Vnlm6'</span>}, {<span class="org-string">'Fnl1'</span>, <span class="org-string">'Fnl2'</span>, <span class="org-string">'Fnl3'</span>, <span class="org-string">'Fnl4'</span>, <span class="org-string">'Fnl5'</span>, <span class="org-string">'Fnl6'</span>}));
+</pre>
+</div>
+
+<p>
+And we save them for further analysis.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">save(<span class="org-string">'./active_damping/mat/undamped_plants.mat'</span>, <span class="org-string">'G_iff'</span>, <span class="org-string">'G_dvf'</span>, <span class="org-string">'G_ine'</span>);
+</pre>
+</div>
+</div>
+</div>
+
+<div id="outline-container-orgc478ad1" class="outline-4">
+<h4 id="orgc478ad1"><span class="section-number-4">1.1.3</span> Obtained Plants for Active Damping</h4>
+<div class="outline-text-4" id="text-1-1-3">
+
+<div id="org26a36fc" class="figure">
+<p><img src="figs/nass_active_damping_iff_plant.png" alt="nass_active_damping_iff_plant.png" />
+</p>
+<p><span class="figure-number">Figure 1: </span><code>G_iff</code>: IFF Plant (<a href="./figs/nass_active_damping_iff_plant.png">png</a>, <a href="./figs/nass_active_damping_iff_plant.pdf">pdf</a>)</p>
+</div>
+
+
+<div id="orgb866959" class="figure">
+<p><img src="figs/nass_active_damping_ine_plant.png" alt="nass_active_damping_ine_plant.png" />
+</p>
+<p><span class="figure-number">Figure 2: </span><code>G_dvf</code>: Plant for Direct Velocity Feedback (<a href="./figs/nass_active_damping_dvf_plant.png">png</a>, <a href="./figs/nass_active_damping_dvf_plant.pdf">pdf</a>)</p>
+</div>
+
+
+<div id="orgbeb0fe2" class="figure">
+<p><img src="figs/nass_active_damping_inertial_plant.png" alt="nass_active_damping_inertial_plant.png" />
+</p>
+<p><span class="figure-number">Figure 3: </span>Inertial Feedback Plant (<a href="./figs/nass_active_damping_inertial_plant.png">png</a>, <a href="./figs/nass_active_damping_inertial_plant.pdf">pdf</a>)</p>
+</div>
+</div>
+</div>
+</div>
+
+<div id="outline-container-org22150e6" class="outline-3">
+<h3 id="org22150e6"><span class="section-number-3">1.2</span> Tomography Experiment</h3>
 <div class="outline-text-3" id="text-1-2">
+</div>
+<div id="outline-container-org931bbc3" class="outline-4">
+<h4 id="org931bbc3"><span class="section-number-4">1.2.1</span> Simulation</h4>
+<div class="outline-text-4" id="text-1-2-1">
 <p>
-We identify the various transfer functions of the system
+We initialize elements for the tomography experiment.
 </p>
 <div class="org-src-container">
-<pre class="src src-matlab">G = identifyPlant<span class="org-rainbow-delimiters-depth-1">()</span>;
+<pre class="src src-matlab">prepareTomographyExperiment();
 </pre>
 </div>
 
 <p>
-And we save it for further analysis.
+We change the simulation stop time.
 </p>
 <div class="org-src-container">
-<pre class="src src-matlab">save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./active_damping/mat/plants.mat'</span>, <span class="org-string">'G'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
+<pre class="src src-matlab">load(<span class="org-string">'mat/conf_simscape.mat'</span>);
+<span class="org-matlab-simulink-keyword">set_param</span>(<span class="org-variable-name">conf_simscape</span>, <span class="org-string">'StopTime'</span>, <span class="org-string">'3'</span>);
+</pre>
+</div>
+
+<p>
+And we simulate the system.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">sim</span>(<span class="org-string">'sim_nass_active_damping'</span>);
+</pre>
+</div>
+
+<p>
+Finally, we save the simulation results for further analysis
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">save(<span class="org-string">'./active_damping/mat/tomo_exp.mat'</span>, <span class="org-string">'En'</span>, <span class="org-string">'Eg'</span>, <span class="org-string">'-append'</span>);
 </pre>
 </div>
 </div>
 </div>
 
-<div id="outline-container-orge1d51ce" class="outline-3">
-<h3 id="orge1d51ce"><span class="section-number-3">1.3</span> Sensitivity to disturbances</h3>
-<div class="outline-text-3" id="text-1-3">
+<div id="outline-container-org97e15e5" class="outline-4">
+<h4 id="org97e15e5"><span class="section-number-4">1.2.2</span> Results</h4>
+<div class="outline-text-4" id="text-1-2-2">
 <p>
-The sensitivity to disturbances are shown on figure <a href="#org3e75ea5">1</a>.
+We load the results of tomography experiments.
 </p>
+<div class="org-src-container">
+<pre class="src src-matlab">load(<span class="org-string">'./active_damping/mat/tomo_exp.mat'</span>, <span class="org-string">'En'</span>);
+t = linspace(0, 3, length(En(<span class="org-type">:</span>,1)));
+</pre>
+</div>
 
 
-<div id="org3e75ea5" class="figure">
-<p><img src="figs/sensitivity_dist_undamped.png" alt="sensitivity_dist_undamped.png" />
+<div id="orge980ab1" class="figure">
+<p><img src="figs/nass_act_damp_undamped_sim_tomo_trans.png" alt="nass_act_damp_undamped_sim_tomo_trans.png" />
 </p>
-<p><span class="figure-number">Figure 1: </span>Undamped sensitivity to disturbances (<a href="./figs/sensitivity_dist_undamped.png">png</a>, <a href="./figs/sensitivity_dist_undamped.pdf">pdf</a>)</p>
+<p><span class="figure-number">Figure 4: </span>Position Error during tomography experiment - Translations (<a href="./figs/nass_act_damp_undamped_sim_tomo_trans.png">png</a>, <a href="./figs/nass_act_damp_undamped_sim_tomo_trans.pdf">pdf</a>)</p>
 </div>
 
 
-<div id="orgf075746" class="figure">
-<p><img src="figs/sensitivity_dist_stages.png" alt="sensitivity_dist_stages.png" />
+<div id="org669dfe3" class="figure">
+<p><img src="figs/nass_act_damp_undamped_sim_tomo_rot.png" alt="nass_act_damp_undamped_sim_tomo_rot.png" />
 </p>
-<p><span class="figure-number">Figure 2: </span>Sensitivity to force disturbances in various stages (<a href="./figs/sensitivity_dist_stages.png">png</a>, <a href="./figs/sensitivity_dist_stages.pdf">pdf</a>)</p>
+<p><span class="figure-number">Figure 5: </span>Position Error during tomography experiment - Rotations (<a href="./figs/nass_act_damp_undamped_sim_tomo_rot.png">png</a>, <a href="./figs/nass_act_damp_undamped_sim_tomo_rot.pdf">pdf</a>)</p>
 </div>
 </div>
 </div>
-
-<div id="outline-container-org58ffec6" class="outline-3">
-<h3 id="org58ffec6"><span class="section-number-3">1.4</span> Undamped Plant</h3>
-<div class="outline-text-3" id="text-1-4">
-<p>
-The "plant" (transfer function from forces applied by the nano-hexapod to the measured displacement of the sample with respect to the granite) bode plot is shown on figure <a href="#org3e75ea5">1</a>.
-</p>
-
-
-<div id="org2f56118" class="figure">
-<p><img src="figs/plant_undamped.png" alt="plant_undamped.png" />
-</p>
-<p><span class="figure-number">Figure 3: </span>Transfer Function from cartesian forces to displacement for the undamped plant (<a href="./figs/plant_undamped.png">png</a>, <a href="./figs/plant_undamped.pdf">pdf</a>)</p>
-</div>
-</div>
 </div>
 </div>
 
-<div id="outline-container-orgf769f9d" class="outline-2">
-<h2 id="orgf769f9d"><span class="section-number-2">2</span> Integral Force Feedback</h2>
+<div id="outline-container-orgeca85c7" class="outline-2">
+<h2 id="orgeca85c7"><span class="section-number-2">2</span> Integral Force Feedback</h2>
 <div class="outline-text-2" id="text-2">
 <p>
-<a id="org6c8af88"></a>
+<a id="org45880cb"></a>
 </p>
 <div class="note">
 <p>
@@ -510,186 +651,55 @@ All the files (data and Matlab scripts) are accessible <a href="data/iff.zip">he
 
 </div>
 <p>
-Integral Force Feedback is applied.
-In section <a href="#org2307841">2.1</a>, IFF is applied on a uni-axial system to understand its behavior.
-Then, it is applied on the simscape model.
+Integral Force Feedback is applied on the simscape model.
 </p>
 </div>
 
-<div id="outline-container-orgf0e86b6" class="outline-3">
-<h3 id="orgf0e86b6"><span class="section-number-3">2.1</span> One degree-of-freedom example</h3>
+<div id="outline-container-org8e730eb" class="outline-3">
+<h3 id="org8e730eb"><span class="section-number-3">2.1</span> Control Design</h3>
 <div class="outline-text-3" id="text-2-1">
-<p>
-<a id="org2307841"></a>
-</p>
 </div>
-<div id="outline-container-org00cf95e" class="outline-4">
-<h4 id="org00cf95e"><span class="section-number-4">2.1.1</span> Equations</h4>
+<div id="outline-container-org4aae4a5" class="outline-4">
+<h4 id="org4aae4a5"><span class="section-number-4">2.1.1</span> Plant</h4>
 <div class="outline-text-4" id="text-2-1-1">
-
-<div id="org0cb97db" class="figure">
-<p><img src="figs/iff_1dof.png" alt="iff_1dof.png" />
-</p>
-<p><span class="figure-number">Figure 4: </span>Integral Force Feedback applied to a 1dof system</p>
-</div>
-
 <p>
-The dynamic of the system is described by the following equation:
-</p>
-\begin{equation}
-  ms^2x = F_d - kx - csx + kw + csw + F
-\end{equation}
-<p>
-The measured force \(F_m\) is:
-</p>
-\begin{align}
-  F_m &= F - kx - csx + kw + csw \\
-      &= ms^2 x - F_d
-\end{align}
-<p>
-The Integral Force Feedback controller is \(K = -\frac{g}{s}\), and thus the applied force by this controller is:
-</p>
-\begin{equation}
-  F_{\text{IFF}} = -\frac{g}{s} F_m = -\frac{g}{s} (ms^2 x - F_d)
-\end{equation}
-<p>
-Once the IFF is applied, the new dynamics of the system is:
-</p>
-\begin{equation}
-  ms^2x = F_d + F - kx - csx + kw + csw - \frac{g}{s} (ms^2x - F_d)
-\end{equation}
-
-<p>
-And finally:
-</p>
-\begin{equation}
-  x = F_d \frac{1 + \frac{g}{s}}{ms^2 + (mg + c)s + k} + F \frac{1}{ms^2 + (mg + c)s + k} +  w \frac{k + cs}{ms^2 + (mg + c)s + k}
-\end{equation}
-
-<p>
-We can see that this:
-</p>
-<ul class="org-ul">
-<li>adds damping to the system by a value \(mg\)</li>
-<li>lower the compliance as low frequency by a factor: \(1 + g/s\)</li>
-</ul>
-
-<p>
-If we want critical damping:
-</p>
-\begin{equation}
-  \xi = \frac{1}{2} \frac{c + gm}{\sqrt{km}} = \frac{1}{2}
-\end{equation}
-
-<p>
-This is attainable if we have:
-</p>
-\begin{equation}
-  g = \frac{\sqrt{km} - c}{m}
-\end{equation}
-</div>
-</div>
-
-<div id="outline-container-org227d2a7" class="outline-4">
-<h4 id="org227d2a7"><span class="section-number-4">2.1.2</span> Matlab Example</h4>
-<div class="outline-text-4" id="text-2-1-2">
-<p>
-Let define the system parameters.
+Let&rsquo;s load the previously indentified undamped plant:
 </p>
 <div class="org-src-container">
-<pre class="src src-matlab">m = <span class="org-highlight-numbers-number">50</span>; <span class="org-comment">% [kg]</span>
-k = <span class="org-highlight-numbers-number">1e6</span>; <span class="org-comment">% [N/m]</span>
-c = <span class="org-highlight-numbers-number">1e3</span>; <span class="org-comment">% [N/(m/s)]</span>
+<pre class="src src-matlab">load(<span class="org-string">'./active_damping/mat/undamped_plants.mat'</span>, <span class="org-string">'G_iff'</span>);
 </pre>
 </div>
 
 <p>
-The state space model of the system is defined below.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">A = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-type">-</span>c<span class="org-type">/</span>m <span class="org-type">-</span>k<span class="org-type">/</span>m;
-     <span class="org-highlight-numbers-number">1</span>     <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
-
-B = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span><span class="org-type">/</span>m <span class="org-highlight-numbers-number">1</span><span class="org-type">/</span>m <span class="org-type">-</span><span class="org-highlight-numbers-number">1</span>;
-     <span class="org-highlight-numbers-number">0</span>   <span class="org-highlight-numbers-number">0</span>    <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
-
-C = <span class="org-rainbow-delimiters-depth-1">[</span> <span class="org-highlight-numbers-number">0</span>  <span class="org-highlight-numbers-number">1</span>;
-     <span class="org-type">-</span>c <span class="org-type">-</span>k<span class="org-rainbow-delimiters-depth-1">]</span>;
-
-D = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;
-     <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
-
-sys = ss<span class="org-rainbow-delimiters-depth-1">(</span>A, B, C, D<span class="org-rainbow-delimiters-depth-1">)</span>;
-sys.InputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'F'</span>, <span class="org-string">'Fd'</span>, <span class="org-string">'wddot'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
-sys.OutputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'d'</span>, <span class="org-string">'Fm'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
-sys.StateName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'ddot'</span>, <span class="org-string">'d'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
-</pre>
-</div>
-
-<p>
-The controller \(K_\text{IFF}\) is:
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">Kiff = <span class="org-type">-</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>sqrt<span class="org-rainbow-delimiters-depth-3">(</span>k<span class="org-type">*</span>m<span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">-</span>c<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span>m<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span>s;
-Kiff.InputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'Fm'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
-Kiff.OutputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'F'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
-</pre>
-</div>
-
-<p>
-And the closed loop system is computed below.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">sys_iff = feedback<span class="org-rainbow-delimiters-depth-1">(</span>sys, Kiff, <span class="org-string">'name'</span>, <span class="org-type">+</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-</pre>
-</div>
-
-
-<div id="org7642d37" class="figure">
-<p><img src="figs/iff_1dof_sensitivitiy.png" alt="iff_1dof_sensitivitiy.png" />
-</p>
-<p><span class="figure-number">Figure 5: </span>Sensitivity to disturbance when IFF is applied on the 1dof system (<a href="./figs/iff_1dof_sensitivitiy.png">png</a>, <a href="./figs/iff_1dof_sensitivitiy.pdf">pdf</a>)</p>
-</div>
-</div>
-</div>
-</div>
-
-<div id="outline-container-orga9d4adb" class="outline-3">
-<h3 id="orga9d4adb"><span class="section-number-3">2.2</span> Control Design</h3>
-<div class="outline-text-3" id="text-2-2">
-<p>
-Let's load the undamped plant:
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./active_damping/mat/plants.mat'</span>, <span class="org-string">'G'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-</pre>
-</div>
-
-<p>
-Let's look at the transfer function from actuator forces in the nano-hexapod to the force sensor in the nano-hexapod legs for all 6 pairs of actuator/sensor (figure <a href="#orgdaff411">6</a>).
+Let&rsquo;s look at the transfer function from actuator forces in the nano-hexapod to the force sensor in the nano-hexapod legs for all 6 pairs of actuator/sensor (figure <a href="#orgc1d7801">6</a>).
 </p>
 
 
-<div id="orgdaff411" class="figure">
+<div id="orgc1d7801" class="figure">
 <p><img src="figs/iff_plant.png" alt="iff_plant.png" />
 </p>
 <p><span class="figure-number">Figure 6: </span>Transfer function from forces applied in the legs to force sensor (<a href="./figs/iff_plant.png">png</a>, <a href="./figs/iff_plant.pdf">pdf</a>)</p>
 </div>
+</div>
+</div>
 
+<div id="outline-container-org9230021" class="outline-4">
+<h4 id="org9230021"><span class="section-number-4">2.1.2</span> Control Design</h4>
+<div class="outline-text-4" id="text-2-1-2">
 <p>
 The controller for each pair of actuator/sensor is:
 </p>
 <div class="org-src-container">
-<pre class="src src-matlab">K_iff = <span class="org-type">-</span><span class="org-highlight-numbers-number">1000</span><span class="org-type">/</span>s;
+<pre class="src src-matlab">K_iff = 1000<span class="org-type">/</span>s;
 </pre>
 </div>
 
 <p>
-The corresponding loop gains are shown in figure <a href="#orge16a678">7</a>.
+The corresponding loop gains are shown in figure <a href="#orgb6d4182">7</a>.
 </p>
 
 
-<div id="orge16a678" class="figure">
+<div id="orgb6d4182" class="figure">
 <p><img src="figs/iff_open_loop.png" alt="iff_open_loop.png" />
 </p>
 <p><span class="figure-number">Figure 7: </span>Loop Gain for the Integral Force Feedback (<a href="./figs/iff_open_loop.png">png</a>, <a href="./figs/iff_open_loop.pdf">pdf</a>)</p>
@@ -697,130 +707,123 @@ The corresponding loop gains are shown in figure <a href="#orge16a678">7</a>.
 </div>
 </div>
 
-<div id="outline-container-org582d26c" class="outline-3">
-<h3 id="org582d26c"><span class="section-number-3">2.3</span> Identification of the damped plant</h3>
+<div id="outline-container-orgc48be49" class="outline-4">
+<h4 id="orgc48be49"><span class="section-number-4">2.1.3</span> Diagonal Controller</h4>
+<div class="outline-text-4" id="text-2-1-3">
+<p>
+We create the diagonal controller and we add a minus sign as we have a positive
+feedback architecture.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">K_iff = <span class="org-type">-</span>K_iff<span class="org-type">*</span>eye(6);
+</pre>
+</div>
+
+<p>
+We save the controller for further analysis.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">save(<span class="org-string">'./active_damping/mat/K_iff.mat'</span>, <span class="org-string">'K_iff'</span>);
+</pre>
+</div>
+</div>
+</div>
+</div>
+
+<div id="outline-container-org521b83c" class="outline-3">
+<h3 id="org521b83c"><span class="section-number-3">2.2</span> Tomography Experiment</h3>
+<div class="outline-text-3" id="text-2-2">
+</div>
+<div id="outline-container-org80693d0" class="outline-4">
+<h4 id="org80693d0"><span class="section-number-4">2.2.1</span> Initialize the Simulation</h4>
+<div class="outline-text-4" id="text-2-2-1">
+<p>
+We initialize elements for the tomography experiment.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">prepareTomographyExperiment();
+</pre>
+</div>
+
+<p>
+We set the IFF controller.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">load(<span class="org-string">'./active_damping/mat/K_iff.mat'</span>, <span class="org-string">'K_iff'</span>);
+save(<span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_iff'</span>, <span class="org-string">'-append'</span>);
+</pre>
+</div>
+</div>
+</div>
+
+<div id="outline-container-org828fe0c" class="outline-4">
+<h4 id="org828fe0c"><span class="section-number-4">2.2.2</span> Simulation</h4>
+<div class="outline-text-4" id="text-2-2-2">
+<p>
+We change the simulation stop time.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">load(<span class="org-string">'mat/conf_simscape.mat'</span>);
+<span class="org-matlab-simulink-keyword">set_param</span>(<span class="org-variable-name">conf_simscape</span>, <span class="org-string">'StopTime'</span>, <span class="org-string">'3'</span>);
+</pre>
+</div>
+
+<p>
+And we simulate the system.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">sim</span>(<span class="org-string">'sim_nass_active_damping'</span>);
+</pre>
+</div>
+
+<p>
+Finally, we save the simulation results for further analysis
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">En_iff = En;
+Eg_iff = Eg;
+save(<span class="org-string">'./active_damping/mat/tomo_exp.mat'</span>, <span class="org-string">'En_iff'</span>, <span class="org-string">'Eg_iff'</span>, <span class="org-string">'-append'</span>);
+</pre>
+</div>
+</div>
+</div>
+
+<div id="outline-container-orgba367cd" class="outline-4">
+<h4 id="orgba367cd"><span class="section-number-4">2.2.3</span> Compare with Undamped system</h4>
+<div class="outline-text-4" id="text-2-2-3">
+<p>
+We load the results of tomography experiments.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">load(<span class="org-string">'./active_damping/mat/tomo_exp.mat'</span>, <span class="org-string">'En'</span>, <span class="org-string">'En_iff'</span>);
+</pre>
+</div>
+
+<div class="org-src-container">
+<pre class="src src-matlab">t = linspace(0, 3, length(En(<span class="org-type">:</span>,1)));
+</pre>
+</div>
+
+
+<div id="org8a913a4" class="figure">
+<p><img src="figs/nass_act_damp_iff_sim_tomo_trans.png" alt="nass_act_damp_iff_sim_tomo_trans.png" />
+</p>
+<p><span class="figure-number">Figure 8: </span>Position Error during tomography experiment - Translations (<a href="./figs/nass_act_damp_iff_sim_tomo_trans.png">png</a>, <a href="./figs/nass_act_damp_iff_sim_tomo_trans.pdf">pdf</a>)</p>
+</div>
+
+
+<div id="org5a68859" class="figure">
+<p><img src="figs/nass_act_damp_iff_sim_tomo_rot.png" alt="nass_act_damp_iff_sim_tomo_rot.png" />
+</p>
+<p><span class="figure-number">Figure 9: </span>Position Error during tomography experiment - Rotations (<a href="./figs/nass_act_damp_iff_sim_tomo_rot.png">png</a>, <a href="./figs/nass_act_damp_iff_sim_tomo_rot.pdf">pdf</a>)</p>
+</div>
+</div>
+</div>
+</div>
+
+<div id="outline-container-org64c89c0" class="outline-3">
+<h3 id="org64c89c0"><span class="section-number-3">2.3</span> Conclusion</h3>
 <div class="outline-text-3" id="text-2-3">
-<p>
-Let's initialize the system prior to identification.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">initializeReferences<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeGround<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeGranite<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeTy<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeRy<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeRz<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeMicroHexapod<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeAxisc<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeMirror<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeNanoHexapod<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'actuator'</span>, <span class="org-string">'piezo'</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-initializeSample<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'mass'</span>, <span class="org-highlight-numbers-number">50</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-</pre>
-</div>
-
-<p>
-All the controllers are set to 0.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">K = tf<span class="org-rainbow-delimiters-depth-1">(</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-K_iff = <span class="org-type">-</span>K_iff<span class="org-type">*</span>eye<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_iff'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-K_rmc = tf<span class="org-rainbow-delimiters-depth-1">(</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_rmc'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-K_dvf = tf<span class="org-rainbow-delimiters-depth-1">(</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_dvf'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-</pre>
-</div>
-
-<p>
-We identify the system dynamics now that the IFF controller is ON.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">G_iff = identifyPlant<span class="org-rainbow-delimiters-depth-1">()</span>;
-</pre>
-</div>
-
-<p>
-And we save the damped plant for further analysis
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./active_damping/mat/plants.mat'</span>, <span class="org-string">'G_iff'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-</pre>
-</div>
-</div>
-</div>
-
-<div id="outline-container-org231207b" class="outline-3">
-<h3 id="org231207b"><span class="section-number-3">2.4</span> Sensitivity to disturbances</h3>
-<div class="outline-text-3" id="text-2-4">
-<p>
-As shown on figure <a href="#org9e80cba">8</a>:
-</p>
-<ul class="org-ul">
-<li>The top platform of the nano-hexapod how behaves as a "free-mass".</li>
-<li>The transfer function from direct forces \(F_s\) to the relative displacement \(D\) is equivalent to the one of an isolated mass.</li>
-<li>The transfer function from ground motion \(D_g\) to the relative displacement \(D\) tends to the transfer function from \(D_g\) to the displacement of the granite (the sample is being isolated thanks to IFF).
-However, as the goal is to make the relative displacement \(D\) as small as possible (e.g. to make the sample motion follows the granite motion), this is not a good thing.</li>
-</ul>
-
-
-<div id="org9e80cba" class="figure">
-<p><img src="figs/sensitivity_dist_iff.png" alt="sensitivity_dist_iff.png" />
-</p>
-<p><span class="figure-number">Figure 8: </span>Sensitivity to disturbance once the IFF controller is applied to the system (<a href="./figs/sensitivity_dist_iff.png">png</a>, <a href="./figs/sensitivity_dist_iff.pdf">pdf</a>)</p>
-</div>
-
-<div class="warning">
-<p>
-The order of the models are very high and thus the plots may be wrong.
-For instance, the plots are not the same when using <code>minreal</code>.
-</p>
-
-</div>
-
-
-<div id="org75ad751" class="figure">
-<p><img src="figs/sensitivity_dist_stages_iff.png" alt="sensitivity_dist_stages_iff.png" />
-</p>
-<p><span class="figure-number">Figure 9: </span>Sensitivity to force disturbances in various stages when IFF is applied (<a href="./figs/sensitivity_dist_stages_iff.png">png</a>, <a href="./figs/sensitivity_dist_stages_iff.pdf">pdf</a>)</p>
-</div>
-</div>
-</div>
-
-<div id="outline-container-org9157c29" class="outline-3">
-<h3 id="org9157c29"><span class="section-number-3">2.5</span> Damped Plant</h3>
-<div class="outline-text-3" id="text-2-5">
-<p>
-Now, look at the new damped plant to control.
-</p>
-
-<p>
-It damps the plant (resonance of the nano hexapod as well as other resonances) as shown in figure <a href="#org9c69a8b">10</a>.
-</p>
-
-
-<div id="org9c69a8b" class="figure">
-<p><img src="figs/plant_iff_damped.png" alt="plant_iff_damped.png" />
-</p>
-<p><span class="figure-number">Figure 10: </span>Damped Plant after IFF is applied (<a href="./figs/plant_iff_damped.png">png</a>, <a href="./figs/plant_iff_damped.pdf">pdf</a>)</p>
-</div>
-
-<p>
-However, it increases coupling at low frequency (figure <a href="#org612b8f4">11</a>).
-</p>
-
-<div id="org612b8f4" class="figure">
-<p><img src="figs/plant_iff_coupling.png" alt="plant_iff_coupling.png" />
-</p>
-<p><span class="figure-number">Figure 11: </span>Coupling induced by IFF (<a href="./figs/plant_iff_coupling.png">png</a>, <a href="./figs/plant_iff_coupling.pdf">pdf</a>)</p>
-</div>
-</div>
-</div>
-
-<div id="outline-container-org79a4305" class="outline-3">
-<h3 id="org79a4305"><span class="section-number-3">2.6</span> Conclusion</h3>
-<div class="outline-text-3" id="text-2-6">
 <div class="important">
 <p>
 Integral Force Feedback:
@@ -836,293 +839,192 @@ Integral Force Feedback:
 </div>
 </div>
 
-<div id="outline-container-org56506d8" class="outline-2">
-<h2 id="org56506d8"><span class="section-number-2">3</span> Relative Motion Control</h2>
+<div id="outline-container-org1bec1c8" class="outline-2">
+<h2 id="org1bec1c8"><span class="section-number-2">3</span> Direct Velocity Feedback</h2>
 <div class="outline-text-2" id="text-3">
 <p>
-<a id="org5aaa6c2"></a>
+<a id="org88df20b"></a>
 </p>
 <div class="note">
 <p>
-All the files (data and Matlab scripts) are accessible <a href="data/rmc.zip">here</a>.
+All the files (data and Matlab scripts) are accessible <a href="data/dvf.zip">here</a>.
 </p>
 
 </div>
 <p>
-In the Relative Motion Control (RMC), a derivative feedback is applied between the measured actuator displacement to the actuator force input.
+In the Direct Velocity Feedback (DVF), a derivative feedback is applied between the measured actuator displacement to the actuator force input.
+The actuator displacement can be measured with a capacitive sensor for instance.
 </p>
 </div>
 
-<div id="outline-container-org6502b14" class="outline-3">
-<h3 id="org6502b14"><span class="section-number-3">3.1</span> One degree-of-freedom example</h3>
+<div id="outline-container-org34fcfd6" class="outline-3">
+<h3 id="org34fcfd6"><span class="section-number-3">3.1</span> Control Design</h3>
 <div class="outline-text-3" id="text-3-1">
-<p>
-<a id="orgfb4a669"></a>
-</p>
 </div>
-<div id="outline-container-orga4b0d18" class="outline-4">
-<h4 id="orga4b0d18"><span class="section-number-4">3.1.1</span> Equations</h4>
+<div id="outline-container-org2e5695f" class="outline-4">
+<h4 id="org2e5695f"><span class="section-number-4">3.1.1</span> Plant</h4>
 <div class="outline-text-4" id="text-3-1-1">
-
-<div id="orge0b1b15" class="figure">
-<p><img src="figs/rmc_1dof.png" alt="rmc_1dof.png" />
+<p>
+Let&rsquo;s load the undamped plant:
 </p>
-<p><span class="figure-number">Figure 12: </span>Relative Motion Control applied to a 1dof system</p>
+<div class="org-src-container">
+<pre class="src src-matlab">load(<span class="org-string">'./active_damping/mat/undamped_plants.mat'</span>, <span class="org-string">'G_dvf'</span>);
+</pre>
 </div>
 
 <p>
-The dynamic of the system is:
-</p>
-\begin{equation}
-  ms^2x = F_d - kx - csx + kw + csw + F
-\end{equation}
-<p>
-In terms of the stage deformation \(d = x - w\):
-</p>
-\begin{equation}
-  (ms^2 + cs + k) d = -ms^2 w + F_d + F
-\end{equation}
-<p>
-The relative motion control law is:
-</p>
-\begin{equation}
-  K = -g s
-\end{equation}
-<p>
-Thus, the applied force is:
-</p>
-\begin{equation}
-  F = -g s d
-\end{equation}
-<p>
-And the new dynamics will be:
-</p>
-\begin{equation}
-  d = w \frac{-ms^2}{ms^2 + (c + g)s + k} + F_d \frac{1}{ms^2 + (c + g)s + k} + F \frac{1}{ms^2 + (c + g)s + k}
-\end{equation}
-
-<p>
-And thus damping is added.
+Let&rsquo;s look at the transfer function from actuator forces in the nano-hexapod to the measured displacement of the actuator for all 6 pairs of actuator/sensor (figure <a href="#org7fbb7c6">10</a>).
 </p>
 
-<p>
-If critical damping is wanted:
+
+<div id="org7fbb7c6" class="figure">
+<p><img src="figs/dvf_plant.png" alt="dvf_plant.png" />
 </p>
-\begin{equation}
-  \xi = \frac{1}{2}\frac{c + g}{\sqrt{km}} = \frac{1}{2}
-\end{equation}
-<p>
-This corresponds to a gain:
-</p>
-\begin{equation}
-  g = \sqrt{km} - c
-\end{equation}
+<p><span class="figure-number">Figure 10: </span>Transfer function from forces applied in the legs to leg displacement sensor (<a href="./figs/dvf_plant.png">png</a>, <a href="./figs/dvf_plant.pdf">pdf</a>)</p>
+</div>
 </div>
 </div>
 
-<div id="outline-container-org6043cef" class="outline-4">
-<h4 id="org6043cef"><span class="section-number-4">3.1.2</span> Matlab Example</h4>
+<div id="outline-container-org8feed7c" class="outline-4">
+<h4 id="org8feed7c"><span class="section-number-4">3.1.2</span> Control Design</h4>
 <div class="outline-text-4" id="text-3-1-2">
 <p>
-Let define the system parameters.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">m = <span class="org-highlight-numbers-number">50</span>; <span class="org-comment">% [kg]</span>
-k = <span class="org-highlight-numbers-number">1e6</span>; <span class="org-comment">% [N/m]</span>
-c = <span class="org-highlight-numbers-number">1e3</span>; <span class="org-comment">% [N/(m/s)]</span>
-</pre>
-</div>
-
-<p>
-The state space model of the system is defined below.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">A = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-type">-</span>c<span class="org-type">/</span>m <span class="org-type">-</span>k<span class="org-type">/</span>m;
-     <span class="org-highlight-numbers-number">1</span>     <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
-
-B = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span><span class="org-type">/</span>m <span class="org-highlight-numbers-number">1</span><span class="org-type">/</span>m <span class="org-type">-</span><span class="org-highlight-numbers-number">1</span>;
-     <span class="org-highlight-numbers-number">0</span>   <span class="org-highlight-numbers-number">0</span>    <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
-
-C = <span class="org-rainbow-delimiters-depth-1">[</span> <span class="org-highlight-numbers-number">0</span>  <span class="org-highlight-numbers-number">1</span>;
-     <span class="org-type">-</span>c <span class="org-type">-</span>k<span class="org-rainbow-delimiters-depth-1">]</span>;
-
-D = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;
-     <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
-
-sys = ss<span class="org-rainbow-delimiters-depth-1">(</span>A, B, C, D<span class="org-rainbow-delimiters-depth-1">)</span>;
-sys.InputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'F'</span>, <span class="org-string">'Fd'</span>, <span class="org-string">'wddot'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
-sys.OutputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'d'</span>, <span class="org-string">'Fm'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
-sys.StateName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'ddot'</span>, <span class="org-string">'d'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
-</pre>
-</div>
-
-<p>
-The controller \(K_\text{RMC}\) is:
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">Krmc = <span class="org-type">-</span><span class="org-rainbow-delimiters-depth-1">(</span>sqrt<span class="org-rainbow-delimiters-depth-2">(</span>k<span class="org-type">*</span>m<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">-</span>c<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span>s;
-Krmc.InputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'d'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
-Krmc.OutputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'F'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
-</pre>
-</div>
-
-<p>
-And the closed loop system is computed below.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">sys_rmc = feedback<span class="org-rainbow-delimiters-depth-1">(</span>sys, Krmc, <span class="org-string">'name'</span>, <span class="org-type">+</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-</pre>
-</div>
-
-
-<div id="org6f81ea6" class="figure">
-<p><img src="figs/rmc_1dof_sensitivitiy.png" alt="rmc_1dof_sensitivitiy.png" />
-</p>
-<p><span class="figure-number">Figure 13: </span>Sensitivity to disturbance when RMC is applied on the 1dof system (<a href="./figs/rmc_1dof_sensitivitiy.png">png</a>, <a href="./figs/rmc_1dof_sensitivitiy.pdf">pdf</a>)</p>
-</div>
-</div>
-</div>
-</div>
-
-<div id="outline-container-orgdf908e5" class="outline-3">
-<h3 id="orgdf908e5"><span class="section-number-3">3.2</span> Control Design</h3>
-<div class="outline-text-3" id="text-3-2">
-<p>
-Let's load the undamped plant:
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./active_damping/mat/plants.mat'</span>, <span class="org-string">'G'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-</pre>
-</div>
-
-<p>
-Let's look at the transfer function from actuator forces in the nano-hexapod to the measured displacement of the actuator for all 6 pairs of actuator/sensor (figure <a href="#org7711c21">14</a>).
-</p>
-
-
-<div id="org7711c21" class="figure">
-<p><img src="figs/rmc_plant.png" alt="rmc_plant.png" />
-</p>
-<p><span class="figure-number">Figure 14: </span>Transfer function from forces applied in the legs to leg displacement sensor (<a href="./figs/rmc_plant.png">png</a>, <a href="./figs/rmc_plant.pdf">pdf</a>)</p>
-</div>
-
-<p>
-The Relative Motion Controller is defined below.
+The Direct Velocity Feedback is defined below.
 A Low pass Filter is added to make the controller transfer function proper.
 </p>
 <div class="org-src-container">
-<pre class="src src-matlab">K_rmc = s<span class="org-type">*</span><span class="org-highlight-numbers-number">50000</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span> <span class="org-type">+</span> s<span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">10000</span><span class="org-rainbow-delimiters-depth-1">)</span>;
+<pre class="src src-matlab">K_dvf = s<span class="org-type">*</span>20000<span class="org-type">/</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">/</span>10000);
 </pre>
 </div>
 
 <p>
-The obtained loop gains are shown in figure <a href="#org5ad6d50">15</a>.
+The obtained loop gains are shown in figure <a href="#orgdcfd988">11</a>.
 </p>
 
 
-<div id="org5ad6d50" class="figure">
-<p><img src="figs/rmc_open_loop.png" alt="rmc_open_loop.png" />
+<div id="orgdcfd988" class="figure">
+<p><img src="figs/dvf_open_loop.png" alt="dvf_open_loop.png" />
 </p>
-<p><span class="figure-number">Figure 15: </span>Loop Gain for the Integral Force Feedback (<a href="./figs/rmc_open_loop.png">png</a>, <a href="./figs/rmc_open_loop.pdf">pdf</a>)</p>
+<p><span class="figure-number">Figure 11: </span>Loop Gain for the Integral Force Feedback (<a href="./figs/dvf_open_loop.png">png</a>, <a href="./figs/dvf_open_loop.pdf">pdf</a>)</p>
 </div>
 </div>
 </div>
 
-<div id="outline-container-org9792b77" class="outline-3">
-<h3 id="org9792b77"><span class="section-number-3">3.3</span> Identification of the damped plant</h3>
+<div id="outline-container-org5d88841" class="outline-4">
+<h4 id="org5d88841"><span class="section-number-4">3.1.3</span> Diagonal Controller</h4>
+<div class="outline-text-4" id="text-3-1-3">
+<p>
+We create the diagonal controller and we add a minus sign as we have a positive feedback architecture.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">K_dvf = <span class="org-type">-</span>K_dvf<span class="org-type">*</span>eye(6);
+</pre>
+</div>
+
+<p>
+We save the controller for further analysis.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">save(<span class="org-string">'./active_damping/mat/K_dvf.mat'</span>, <span class="org-string">'K_dvf'</span>);
+</pre>
+</div>
+</div>
+</div>
+</div>
+
+<div id="outline-container-orgd0cf173" class="outline-3">
+<h3 id="orgd0cf173"><span class="section-number-3">3.2</span> Tomography Experiment</h3>
+<div class="outline-text-3" id="text-3-2">
+</div>
+<div id="outline-container-org41f51f2" class="outline-4">
+<h4 id="org41f51f2"><span class="section-number-4">3.2.1</span> Initialize the Simulation</h4>
+<div class="outline-text-4" id="text-3-2-1">
+<p>
+We initialize elements for the tomography experiment.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">prepareTomographyExperiment();
+</pre>
+</div>
+
+<p>
+We set the DVF controller.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">load(<span class="org-string">'./active_damping/mat/K_dvf.mat'</span>, <span class="org-string">'K_dvf'</span>);
+save(<span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_dvf'</span>, <span class="org-string">'-append'</span>);
+</pre>
+</div>
+</div>
+</div>
+
+<div id="outline-container-org0da77b8" class="outline-4">
+<h4 id="org0da77b8"><span class="section-number-4">3.2.2</span> Simulation</h4>
+<div class="outline-text-4" id="text-3-2-2">
+<p>
+We change the simulation stop time.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">load(<span class="org-string">'mat/conf_simscape.mat'</span>);
+<span class="org-matlab-simulink-keyword">set_param</span>(<span class="org-variable-name">conf_simscape</span>, <span class="org-string">'StopTime'</span>, <span class="org-string">'3'</span>);
+</pre>
+</div>
+
+<p>
+And we simulate the system.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">sim</span>(<span class="org-string">'sim_nass_active_damping'</span>);
+</pre>
+</div>
+
+<p>
+Finally, we save the simulation results for further analysis
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">En_dvf = En;
+Eg_dvf = Eg;
+save(<span class="org-string">'./active_damping/mat/tomo_exp.mat'</span>, <span class="org-string">'En_dvf'</span>, <span class="org-string">'Eg_dvf'</span>, <span class="org-string">'-append'</span>);
+</pre>
+</div>
+</div>
+</div>
+
+<div id="outline-container-org8956847" class="outline-4">
+<h4 id="org8956847"><span class="section-number-4">3.2.3</span> Compare with Undamped system</h4>
+<div class="outline-text-4" id="text-3-2-3">
+<p>
+We load the results of tomography experiments.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">load(<span class="org-string">'./active_damping/mat/tomo_exp.mat'</span>, <span class="org-string">'En'</span>, <span class="org-string">'En_dvf'</span>);
+t = linspace(0, 3, length(En(<span class="org-type">:</span>,1)));
+</pre>
+</div>
+
+
+<div id="orgd62f9ce" class="figure">
+<p><img src="figs/nass_act_damp_dvf_sim_tomo_trans.png" alt="nass_act_damp_dvf_sim_tomo_trans.png" />
+</p>
+<p><span class="figure-number">Figure 12: </span>Position Error during tomography experiment - Translations (<a href="./figs/nass_act_damp_dvf_sim_tomo_trans.png">png</a>, <a href="./figs/nass_act_damp_dvf_sim_tomo_trans.pdf">pdf</a>)</p>
+</div>
+
+
+<div id="org8213265" class="figure">
+<p><img src="figs/nass_act_damp_dvf_sim_tomo_rot.png" alt="nass_act_damp_dvf_sim_tomo_rot.png" />
+</p>
+<p><span class="figure-number">Figure 13: </span>Position Error during tomography experiment - Rotations (<a href="./figs/nass_act_damp_dvf_sim_tomo_rot.png">png</a>, <a href="./figs/nass_act_damp_dvf_sim_tomo_rot.pdf">pdf</a>)</p>
+</div>
+</div>
+</div>
+</div>
+
+<div id="outline-container-org4a46d6a" class="outline-3">
+<h3 id="org4a46d6a"><span class="section-number-3">3.3</span> Conclusion</h3>
 <div class="outline-text-3" id="text-3-3">
-<p>
-Let's initialize the system prior to identification.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">initializeReferences<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeGround<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeGranite<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeTy<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeRy<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeRz<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeMicroHexapod<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeAxisc<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeMirror<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeNanoHexapod<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'actuator'</span>, <span class="org-string">'piezo'</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-initializeSample<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'mass'</span>, <span class="org-highlight-numbers-number">50</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-</pre>
-</div>
-
-<p>
-And initialize the controllers.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">K = tf<span class="org-rainbow-delimiters-depth-1">(</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-K_iff = tf<span class="org-rainbow-delimiters-depth-1">(</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_iff'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-K_rmc = <span class="org-type">-</span>K_rmc<span class="org-type">*</span>eye<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_rmc'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-K_dvf = tf<span class="org-rainbow-delimiters-depth-1">(</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_dvf'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-</pre>
-</div>
-
-<p>
-We identify the system dynamics now that the RMC controller is ON.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">G_rmc = identifyPlant<span class="org-rainbow-delimiters-depth-1">()</span>;
-</pre>
-</div>
-
-<p>
-And we save the damped plant for further analysis.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./active_damping/mat/plants.mat'</span>, <span class="org-string">'G_rmc'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-</pre>
-</div>
-</div>
-</div>
-
-<div id="outline-container-org4894be4" class="outline-3">
-<h3 id="org4894be4"><span class="section-number-3">3.4</span> Sensitivity to disturbances</h3>
-<div class="outline-text-3" id="text-3-4">
-<p>
-As shown in figure <a href="#org2e950af">16</a>, RMC control succeed in lowering the sensitivity to disturbances near resonance of the system.
-</p>
-
-
-<div id="org2e950af" class="figure">
-<p><img src="figs/sensitivity_dist_rmc.png" alt="sensitivity_dist_rmc.png" />
-</p>
-<p><span class="figure-number">Figure 16: </span>Sensitivity to disturbance once the RMC controller is applied to the system (<a href="./figs/sensitivity_dist_rmc.png">png</a>, <a href="./figs/sensitivity_dist_rmc.pdf">pdf</a>)</p>
-</div>
-
-
-<div id="org302ab8e" class="figure">
-<p><img src="figs/sensitivity_dist_stages_rmc.png" alt="sensitivity_dist_stages_rmc.png" />
-</p>
-<p><span class="figure-number">Figure 17: </span>Sensitivity to force disturbances in various stages when RMC is applied (<a href="./figs/sensitivity_dist_stages_rmc.png">png</a>, <a href="./figs/sensitivity_dist_stages_rmc.pdf">pdf</a>)</p>
-</div>
-</div>
-</div>
-
-<div id="outline-container-orgd89441e" class="outline-3">
-<h3 id="orgd89441e"><span class="section-number-3">3.5</span> Damped Plant</h3>
-<div class="outline-text-3" id="text-3-5">
-
-<div id="orgcead87e" class="figure">
-<p><img src="figs/plant_rmc_damped.png" alt="plant_rmc_damped.png" />
-</p>
-<p><span class="figure-number">Figure 18: </span>Damped Plant after RMC is applied (<a href="./figs/plant_rmc_damped.png">png</a>, <a href="./figs/plant_rmc_damped.pdf">pdf</a>)</p>
-</div>
-</div>
-</div>
-
-<div id="outline-container-org04f7d21" class="outline-3">
-<h3 id="org04f7d21"><span class="section-number-3">3.6</span> Conclusion</h3>
-<div class="outline-text-3" id="text-3-6">
 <div class="important">
 <p>
-Relative Motion Control:
+Direct Velocity Feedback:
 </p>
 <ul class="org-ul">
 <li></li>
@@ -1133,310 +1035,187 @@ Relative Motion Control:
 </div>
 </div>
 
-<div id="outline-container-org9af791d" class="outline-2">
-<h2 id="org9af791d"><span class="section-number-2">4</span> Direct Velocity Feedback</h2>
+<div id="outline-container-orgf8012d0" class="outline-2">
+<h2 id="orgf8012d0"><span class="section-number-2">4</span> Inertial Control</h2>
 <div class="outline-text-2" id="text-4">
 <p>
-<a id="org6b8a067"></a>
+<a id="orgcb7853a"></a>
 </p>
 <div class="note">
 <p>
-All the files (data and Matlab scripts) are accessible <a href="data/dvf.zip">here</a>.
+All the files (data and Matlab scripts) are accessible <a href="data/ine.zip">here</a>.
 </p>
 
 </div>
 <p>
-In the Relative Motion Control (RMC), a feedback is applied between the measured velocity of the platform to the actuator force input.
+In Inertial Control, a feedback is applied between the measured <b>absolute</b> velocity of the platform to the actuator force input.
 </p>
 </div>
 
-<div id="outline-container-orgda71fbe" class="outline-3">
-<h3 id="orgda71fbe"><span class="section-number-3">4.1</span> One degree-of-freedom example</h3>
+<div id="outline-container-org47e2207" class="outline-3">
+<h3 id="org47e2207"><span class="section-number-3">4.1</span> Control Design</h3>
 <div class="outline-text-3" id="text-4-1">
-<p>
-<a id="org8de0a68"></a>
-</p>
 </div>
-<div id="outline-container-org332c25e" class="outline-4">
-<h4 id="org332c25e"><span class="section-number-4">4.1.1</span> Equations</h4>
+<div id="outline-container-orga1c06d5" class="outline-4">
+<h4 id="orga1c06d5"><span class="section-number-4">4.1.1</span> Plant</h4>
 <div class="outline-text-4" id="text-4-1-1">
-
-<div id="org6423df8" class="figure">
-<p><img src="figs/dvf_1dof.png" alt="dvf_1dof.png" />
+<p>
+Let&rsquo;s load the undamped plant:
 </p>
-<p><span class="figure-number">Figure 19: </span>Direct Velocity Feedback applied to a 1dof system</p>
+<div class="org-src-container">
+<pre class="src src-matlab">load(<span class="org-string">'./active_damping/mat/undamped_plants.mat'</span>, <span class="org-string">'G_ine'</span>);
+</pre>
 </div>
 
 <p>
-The dynamic of the system is:
-</p>
-\begin{equation}
-  ms^2x = F_d - kx - csx + kw + csw + F
-\end{equation}
-<p>
-In terms of the stage deformation \(d = x - w\):
-</p>
-\begin{equation}
-  (ms^2 + cs + k) d = -ms^2 w + F_d + F
-\end{equation}
-<p>
-The direct velocity feedback law shown in figure <a href="#org6423df8">19</a> is:
-</p>
-\begin{equation}
-  K = -g
-\end{equation}
-<p>
-Thus, the applied force is:
-</p>
-\begin{equation}
-  F = -g \dot{x}
-\end{equation}
-<p>
-And the new dynamics will be:
-</p>
-\begin{equation}
-  d = w \frac{-ms^2 - gs}{ms^2 + (c + g)s + k} + F_d \frac{1}{ms^2 + (c + g)s + k} + F \frac{1}{ms^2 + (c + g)s + k}
-\end{equation}
-
-<p>
-And thus damping is added.
+Let&rsquo;s look at the transfer function from actuator forces in the nano-hexapod to the measured velocity of the nano-hexapod platform in the direction of the corresponding actuator for all 6 pairs of actuator/sensor (figure <a href="#org8ebf5ec">14</a>).
 </p>
 
-<p>
-If critical damping is wanted:
+
+<div id="org8ebf5ec" class="figure">
+<p><img src="figs/ine_plant.png" alt="ine_plant.png" />
 </p>
-\begin{equation}
-  \xi = \frac{1}{2}\frac{c + g}{\sqrt{km}} = \frac{1}{2}
-\end{equation}
-<p>
-This corresponds to a gain:
-</p>
-\begin{equation}
-  g = \sqrt{km} - c
-\end{equation}
+<p><span class="figure-number">Figure 14: </span>Transfer function from forces applied in the legs to leg velocity sensor (<a href="./figs/ine_plant.png">png</a>, <a href="./figs/ine_plant.pdf">pdf</a>)</p>
+</div>
 </div>
 </div>
 
-<div id="outline-container-org94d4a25" class="outline-4">
-<h4 id="org94d4a25"><span class="section-number-4">4.1.2</span> Matlab Example</h4>
+<div id="outline-container-orgee9c27b" class="outline-4">
+<h4 id="orgee9c27b"><span class="section-number-4">4.1.2</span> Control Design</h4>
 <div class="outline-text-4" id="text-4-1-2">
 <p>
-Let define the system parameters.
+The controller is defined below and the obtained loop gain is shown in figure <a href="#orgf0d92b7">15</a>.
+</p>
+
+<div class="org-src-container">
+<pre class="src src-matlab">K_ine = 1e3<span class="org-type">/</span>(1<span class="org-type">+</span>s<span class="org-type">/</span>(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>100));
+</pre>
+</div>
+
+
+<div id="orgf0d92b7" class="figure">
+<p><img src="figs/ine_open_loop_gain.png" alt="ine_open_loop_gain.png" />
+</p>
+<p><span class="figure-number">Figure 15: </span>Loop Gain for Inertial Control (<a href="./figs/ine_open_loop_gain.png">png</a>, <a href="./figs/ine_open_loop_gain.pdf">pdf</a>)</p>
+</div>
+</div>
+</div>
+
+<div id="outline-container-org6241541" class="outline-4">
+<h4 id="org6241541"><span class="section-number-4">4.1.3</span> Diagonal Controller</h4>
+<div class="outline-text-4" id="text-4-1-3">
+<p>
+We create the diagonal controller and we add a minus sign as we have a positive feedback architecture.
 </p>
 <div class="org-src-container">
-<pre class="src src-matlab">m = <span class="org-highlight-numbers-number">50</span>; <span class="org-comment">% [kg]</span>
-k = <span class="org-highlight-numbers-number">1e6</span>; <span class="org-comment">% [N/m]</span>
-c = <span class="org-highlight-numbers-number">1e3</span>; <span class="org-comment">% [N/(m/s)]</span>
+<pre class="src src-matlab">K_ine = <span class="org-type">-</span>K_ine<span class="org-type">*</span>eye(6);
 </pre>
 </div>
 
 <p>
-The state space model of the system is defined below.
+We save the controller for further analysis.
 </p>
 <div class="org-src-container">
-<pre class="src src-matlab">A = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-type">-</span>c<span class="org-type">/</span>m <span class="org-type">-</span>k<span class="org-type">/</span>m;
-     <span class="org-highlight-numbers-number">1</span>     <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
-
-B = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span><span class="org-type">/</span>m <span class="org-highlight-numbers-number">1</span><span class="org-type">/</span>m <span class="org-type">-</span><span class="org-highlight-numbers-number">1</span>;
-     <span class="org-highlight-numbers-number">0</span>   <span class="org-highlight-numbers-number">0</span>    <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
-
-C = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span>;
-     <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span>;
-     <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
-
-D = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;
-     <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;
-     <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;
-
-sys = ss<span class="org-rainbow-delimiters-depth-1">(</span>A, B, C, D<span class="org-rainbow-delimiters-depth-1">)</span>;
-sys.InputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'F'</span>, <span class="org-string">'Fd'</span>, <span class="org-string">'wddot'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
-sys.OutputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'ddot'</span>, <span class="org-string">'d'</span>, <span class="org-string">'wddot'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
-sys.StateName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'ddot'</span>, <span class="org-string">'d'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
+<pre class="src src-matlab">save(<span class="org-string">'./active_damping/mat/K_ine.mat'</span>, <span class="org-string">'K_ine'</span>);
 </pre>
 </div>
-
-<p>
-Because we need \(\dot{x}\) for feedback, we compute it from the outputs
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">G_xdot = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">/</span>s;
-          <span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
-G_xdot.InputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'ddot'</span>, <span class="org-string">'d'</span>, <span class="org-string">'wddot'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
-G_xdot.OutputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'xdot'</span>, <span class="org-string">'d'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
-</pre>
-</div>
-
-<p>
-Finally, the system is described by <code>sys</code> as defined below.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">sys = series<span class="org-rainbow-delimiters-depth-1">(</span>sys, G_xdot, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">2</span> <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">]</span>, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">2</span> <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">]</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-</pre>
-</div>
-
-<p>
-The controller \(K_\text{DVF}\) is:
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">Kdvf = tf<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">-</span><span class="org-rainbow-delimiters-depth-2">(</span>sqrt<span class="org-rainbow-delimiters-depth-3">(</span>k<span class="org-type">*</span>m<span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">-</span>c<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-Kdvf.InputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'xdot'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
-Kdvf.OutputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'F'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
-</pre>
-</div>
-
-<p>
-And the closed loop system is computed below.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">sys_dvf = feedback<span class="org-rainbow-delimiters-depth-1">(</span>sys, Kdvf, <span class="org-string">'name'</span>, <span class="org-type">+</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-</pre>
-</div>
-
-<p>
-The obtained sensitivity to disturbances is shown in figure <a href="#orgc650b55">20</a>.
-</p>
-
-<div id="orgc650b55" class="figure">
-<p><img src="figs/dvf_1dof_sensitivitiy.png" alt="dvf_1dof_sensitivitiy.png" />
-</p>
-<p><span class="figure-number">Figure 20: </span>Sensitivity to disturbance when DVF is applied on the 1dof system (<a href="./figs/dvf_1dof_sensitivitiy.png">png</a>, <a href="./figs/dvf_1dof_sensitivitiy.pdf">pdf</a>)</p>
-</div>
 </div>
 </div>
 </div>
 
-<div id="outline-container-org78afe53" class="outline-3">
-<h3 id="org78afe53"><span class="section-number-3">4.2</span> Control Design</h3>
+<div id="outline-container-org9625401" class="outline-3">
+<h3 id="org9625401"><span class="section-number-3">4.2</span> Tomography Experiment</h3>
 <div class="outline-text-3" id="text-4-2">
+</div>
+<div id="outline-container-orgf3c8835" class="outline-4">
+<h4 id="orgf3c8835"><span class="section-number-4">4.2.1</span> Initialize the Simulation</h4>
+<div class="outline-text-4" id="text-4-2-1">
 <p>
-Let's load the undamped plant:
+We initialize elements for the tomography experiment.
 </p>
 <div class="org-src-container">
-<pre class="src src-matlab">load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./active_damping/mat/plants.mat'</span>, <span class="org-string">'G'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
+<pre class="src src-matlab">prepareTomographyExperiment();
 </pre>
 </div>
 
 <p>
-Let's look at the transfer function from actuator forces in the nano-hexapod to the measured velocity of the nano-hexapod platform in the direction of the corresponding actuator for all 6 pairs of actuator/sensor (figure <a href="#org7709a09">21</a>).
+We set the Inertial controller.
 </p>
+<div class="org-src-container">
+<pre class="src src-matlab">load(<span class="org-string">'./active_damping/mat/K_ine.mat'</span>, <span class="org-string">'K_ine'</span>);
+save(<span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_ine'</span>, <span class="org-string">'-append'</span>);
+</pre>
+</div>
+</div>
+</div>
 
-
-<div id="org7709a09" class="figure">
-<p><img src="figs/dvf_plant.png" alt="dvf_plant.png" />
+<div id="outline-container-org457a68a" class="outline-4">
+<h4 id="org457a68a"><span class="section-number-4">4.2.2</span> Simulation</h4>
+<div class="outline-text-4" id="text-4-2-2">
+<p>
+We change the simulation stop time.
 </p>
-<p><span class="figure-number">Figure 21: </span>Transfer function from forces applied in the legs to leg velocity sensor (<a href="./figs/dvf_plant.png">png</a>, <a href="./figs/dvf_plant.pdf">pdf</a>)</p>
+<div class="org-src-container">
+<pre class="src src-matlab">load(<span class="org-string">'mat/conf_simscape.mat'</span>);
+<span class="org-matlab-simulink-keyword">set_param</span>(<span class="org-variable-name">conf_simscape</span>, <span class="org-string">'StopTime'</span>, <span class="org-string">'3'</span>);
+</pre>
 </div>
 
 <p>
-The controller is defined below and the obtained loop gain is shown in figure <a href="#org8244266">22</a>.
+And we simulate the system.
 </p>
-
 <div class="org-src-container">
-<pre class="src src-matlab">K_dvf = tf<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">3e4</span><span class="org-rainbow-delimiters-depth-1">)</span>;
+<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">sim</span>(<span class="org-string">'sim_nass_active_damping'</span>);
+</pre>
+</div>
+
+<p>
+Finally, we save the simulation results for further analysis
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">En_ine = En;
+Eg_ine = Eg;
+save(<span class="org-string">'./active_damping/mat/tomo_exp.mat'</span>, <span class="org-string">'En_ine'</span>, <span class="org-string">'Eg_ine'</span>, <span class="org-string">'-append'</span>);
+</pre>
+</div>
+</div>
+</div>
+
+<div id="outline-container-org5c8213d" class="outline-4">
+<h4 id="org5c8213d"><span class="section-number-4">4.2.3</span> Compare with Undamped system</h4>
+<div class="outline-text-4" id="text-4-2-3">
+<p>
+We load the results of tomography experiments.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">load(<span class="org-string">'./active_damping/mat/tomo_exp.mat'</span>, <span class="org-string">'En'</span>, <span class="org-string">'En_ine'</span>);
+t = linspace(0, 3, length(En_ine(<span class="org-type">:</span>,1)));
 </pre>
 </div>
 
 
-<div id="org8244266" class="figure">
-<p><img src="figs/dvf_open_loop_gain.png" alt="dvf_open_loop_gain.png" />
+<div id="orga568df8" class="figure">
+<p><img src="figs/nass_act_damp_ine_sim_tomo_trans.png" alt="nass_act_damp_ine_sim_tomo_trans.png" />
 </p>
-<p><span class="figure-number">Figure 22: </span>Loop Gain for DVF (<a href="./figs/dvf_open_loop_gain.png">png</a>, <a href="./figs/dvf_open_loop_gain.pdf">pdf</a>)</p>
+<p><span class="figure-number">Figure 16: </span>Position Error during tomography experiment - Translations (<a href="./figs/nass_act_damp_ine_sim_tomo_trans.png">png</a>, <a href="./figs/nass_act_damp_ine_sim_tomo_trans.pdf">pdf</a>)</p>
+</div>
+
+
+<div id="org90aff48" class="figure">
+<p><img src="figs/nass_act_damp_ine_sim_tomo_rot.png" alt="nass_act_damp_ine_sim_tomo_rot.png" />
+</p>
+<p><span class="figure-number">Figure 17: </span>Position Error during tomography experiment - Rotations (<a href="./figs/nass_act_damp_ine_sim_tomo_rot.png">png</a>, <a href="./figs/nass_act_damp_ine_sim_tomo_rot.pdf">pdf</a>)</p>
+</div>
 </div>
 </div>
 </div>
 
-<div id="outline-container-orgb4d1b92" class="outline-3">
-<h3 id="orgb4d1b92"><span class="section-number-3">4.3</span> Identification of the damped plant</h3>
+<div id="outline-container-org4e39ecb" class="outline-3">
+<h3 id="org4e39ecb"><span class="section-number-3">4.3</span> Conclusion</h3>
 <div class="outline-text-3" id="text-4-3">
-<p>
-Let's initialize the system prior to identification.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">initializeReferences<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeGround<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeGranite<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeTy<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeRy<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeRz<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeMicroHexapod<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeAxisc<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeMirror<span class="org-rainbow-delimiters-depth-1">()</span>;
-initializeNanoHexapod<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'actuator'</span>, <span class="org-string">'piezo'</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-initializeSample<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'mass'</span>, <span class="org-highlight-numbers-number">50</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-</pre>
-</div>
-
-<p>
-And initialize the controllers.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">K = tf<span class="org-rainbow-delimiters-depth-1">(</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-K_iff = tf<span class="org-rainbow-delimiters-depth-1">(</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_iff'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-K_rmc = tf<span class="org-rainbow-delimiters-depth-1">(</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_rmc'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-K_dvf = <span class="org-type">-</span>K_dvf<span class="org-type">*</span>eye<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_dvf'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-</pre>
-</div>
-
-<p>
-We identify the system dynamics now that the RMC controller is ON.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">G_dvf = identifyPlant<span class="org-rainbow-delimiters-depth-1">()</span>;
-</pre>
-</div>
-
-<p>
-And we save the damped plant for further analysis.
-</p>
-<div class="org-src-container">
-<pre class="src src-matlab">save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./active_damping/mat/plants.mat'</span>, <span class="org-string">'G_dvf'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
-</pre>
-</div>
-</div>
-</div>
-
-<div id="outline-container-org01d1d1c" class="outline-3">
-<h3 id="org01d1d1c"><span class="section-number-3">4.4</span> Sensitivity to disturbances</h3>
-<div class="outline-text-3" id="text-4-4">
-
-<div id="org5ffb6d0" class="figure">
-<p><img src="figs/sensitivity_dist_dvf.png" alt="sensitivity_dist_dvf.png" />
-</p>
-<p><span class="figure-number">Figure 23: </span>Sensitivity to disturbance once the DVF controller is applied to the system (<a href="./figs/sensitivity_dist_dvf.png">png</a>, <a href="./figs/sensitivity_dist_dvf.pdf">pdf</a>)</p>
-</div>
-
-
-
-<div id="org29eec9f" class="figure">
-<p><img src="figs/sensitivity_dist_stages_dvf.png" alt="sensitivity_dist_stages_dvf.png" />
-</p>
-<p><span class="figure-number">Figure 24: </span>Sensitivity to force disturbances in various stages when DVF is applied (<a href="./figs/sensitivity_dist_stages_dvf.png">png</a>, <a href="./figs/sensitivity_dist_stages_dvf.pdf">pdf</a>)</p>
-</div>
-</div>
-</div>
-
-<div id="outline-container-orgeb00466" class="outline-3">
-<h3 id="orgeb00466"><span class="section-number-3">4.5</span> Damped Plant</h3>
-<div class="outline-text-3" id="text-4-5">
-
-<div id="org17f042e" class="figure">
-<p><img src="figs/plant_dvf_damped.png" alt="plant_dvf_damped.png" />
-</p>
-<p><span class="figure-number">Figure 25: </span>Damped Plant after DVF is applied (<a href="./figs/plant_dvf_damped.png">png</a>, <a href="./figs/plant_dvf_damped.pdf">pdf</a>)</p>
-</div>
-</div>
-</div>
-
-<div id="outline-container-orgfb13592" class="outline-3">
-<h3 id="orgfb13592"><span class="section-number-3">4.6</span> Conclusion</h3>
-<div class="outline-text-3" id="text-4-6">
 <div class="important">
 <p>
-Direct Velocity Feedback:
+Inertial Control:
 </p>
 
 </div>
@@ -1444,105 +1223,222 @@ Direct Velocity Feedback:
 </div>
 </div>
 
-<div id="outline-container-org8970afd" class="outline-2">
-<h2 id="org8970afd"><span class="section-number-2">5</span> Comparison</h2>
+<div id="outline-container-orgb97eda0" class="outline-2">
+<h2 id="orgb97eda0"><span class="section-number-2">5</span> Comparison</h2>
 <div class="outline-text-2" id="text-5">
 <p>
-<a id="org5e94b5c"></a>
+<a id="org00d859c"></a>
 </p>
 </div>
-<div id="outline-container-orgb3b1324" class="outline-3">
-<h3 id="orgb3b1324"><span class="section-number-3">5.1</span> Load the plants</h3>
+<div id="outline-container-org7e7c35d" class="outline-3">
+<h3 id="org7e7c35d"><span class="section-number-3">5.1</span> Load the plants</h3>
 <div class="outline-text-3" id="text-5-1">
 <div class="org-src-container">
-<pre class="src src-matlab">load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./active_damping/mat/plants.mat'</span>, <span class="org-string">'G'</span>, <span class="org-string">'G_iff'</span>, <span class="org-string">'G_rmc'</span>, <span class="org-string">'G_dvf'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
+<pre class="src src-matlab">load(<span class="org-string">'./active_damping/mat/plants.mat'</span>, <span class="org-string">'G'</span>, <span class="org-string">'G_iff'</span>, <span class="org-string">'G_ine'</span>, <span class="org-string">'G_dvf'</span>);
 </pre>
 </div>
 </div>
 </div>
 
-<div id="outline-container-orga2db271" class="outline-3">
-<h3 id="orga2db271"><span class="section-number-3">5.2</span> Sensitivity to Disturbance</h3>
+<div id="outline-container-org7c0e10c" class="outline-3">
+<h3 id="org7c0e10c"><span class="section-number-3">5.2</span> Sensitivity to Disturbance</h3>
 <div class="outline-text-3" id="text-5-2">
 
-<div id="orgb501019" class="figure">
+<div id="org1f40d6f" class="figure">
 <p><img src="figs/sensitivity_comp_ground_motion_z.png" alt="sensitivity_comp_ground_motion_z.png" />
 </p>
-<p><span class="figure-number">Figure 26: </span>caption (<a href="./figs/sensitivity_comp_ground_motion_z.png">png</a>, <a href="./figs/sensitivity_comp_ground_motion_z.pdf">pdf</a>)</p>
+<p><span class="figure-number">Figure 18: </span>caption (<a href="./figs/sensitivity_comp_ground_motion_z.png">png</a>, <a href="./figs/sensitivity_comp_ground_motion_z.pdf">pdf</a>)</p>
 </div>
 
 
 
-<div id="org7a082ac" class="figure">
+<div id="org489ca09" class="figure">
 <p><img src="figs/sensitivity_comp_direct_forces_z.png" alt="sensitivity_comp_direct_forces_z.png" />
 </p>
-<p><span class="figure-number">Figure 27: </span>caption (<a href="./figs/sensitivity_comp_direct_forces_z.png">png</a>, <a href="./figs/sensitivity_comp_direct_forces_z.pdf">pdf</a>)</p>
+<p><span class="figure-number">Figure 19: </span>caption (<a href="./figs/sensitivity_comp_direct_forces_z.png">png</a>, <a href="./figs/sensitivity_comp_direct_forces_z.pdf">pdf</a>)</p>
 </div>
 
 
-<div id="org65d09c7" class="figure">
+<div id="orgdc0124f" class="figure">
 <p><img src="figs/sensitivity_comp_spindle_z.png" alt="sensitivity_comp_spindle_z.png" />
 </p>
-<p><span class="figure-number">Figure 28: </span>caption (<a href="./figs/sensitivity_comp_spindle_z.png">png</a>, <a href="./figs/sensitivity_comp_spindle_z.pdf">pdf</a>)</p>
+<p><span class="figure-number">Figure 20: </span>caption (<a href="./figs/sensitivity_comp_spindle_z.png">png</a>, <a href="./figs/sensitivity_comp_spindle_z.pdf">pdf</a>)</p>
 </div>
 
 
-<div id="org14eba88" class="figure">
+<div id="orgddca189" class="figure">
 <p><img src="figs/sensitivity_comp_ty_z.png" alt="sensitivity_comp_ty_z.png" />
 </p>
-<p><span class="figure-number">Figure 29: </span>caption (<a href="./figs/sensitivity_comp_ty_z.png">png</a>, <a href="./figs/sensitivity_comp_ty_z.pdf">pdf</a>)</p>
+<p><span class="figure-number">Figure 21: </span>caption (<a href="./figs/sensitivity_comp_ty_z.png">png</a>, <a href="./figs/sensitivity_comp_ty_z.pdf">pdf</a>)</p>
 </div>
 
 
 
-<div id="org20c4230" class="figure">
+<div id="orgba56d42" class="figure">
 <p><img src="figs/sensitivity_comp_ty_x.png" alt="sensitivity_comp_ty_x.png" />
 </p>
-<p><span class="figure-number">Figure 30: </span>caption (<a href="./figs/sensitivity_comp_ty_x.png">png</a>, <a href="./figs/sensitivity_comp_ty_x.pdf">pdf</a>)</p>
+<p><span class="figure-number">Figure 22: </span>caption (<a href="./figs/sensitivity_comp_ty_x.png">png</a>, <a href="./figs/sensitivity_comp_ty_x.pdf">pdf</a>)</p>
 </div>
 </div>
 </div>
 
-<div id="outline-container-org0d6cdc3" class="outline-3">
-<h3 id="org0d6cdc3"><span class="section-number-3">5.3</span> Damped Plant</h3>
+<div id="outline-container-org34d3217" class="outline-3">
+<h3 id="org34d3217"><span class="section-number-3">5.3</span> Damped Plant</h3>
 <div class="outline-text-3" id="text-5-3">
 
-<div id="org19883c2" class="figure">
+<div id="org14da053" class="figure">
 <p><img src="figs/plant_comp_damping_z.png" alt="plant_comp_damping_z.png" />
 </p>
-<p><span class="figure-number">Figure 31: </span>Plant for the \(z\) direction for different active damping technique used (<a href="./figs/plant_comp_damping_z.png">png</a>, <a href="./figs/plant_comp_damping_z.pdf">pdf</a>)</p>
+<p><span class="figure-number">Figure 23: </span>Plant for the \(z\) direction for different active damping technique used (<a href="./figs/plant_comp_damping_z.png">png</a>, <a href="./figs/plant_comp_damping_z.pdf">pdf</a>)</p>
 </div>
 
 
-<div id="orgda8ddad" class="figure">
+<div id="org1eac68b" class="figure">
 <p><img src="figs/plant_comp_damping_x.png" alt="plant_comp_damping_x.png" />
 </p>
-<p><span class="figure-number">Figure 32: </span>Plant for the \(x\) direction for different active damping technique used (<a href="./figs/plant_comp_damping_x.png">png</a>, <a href="./figs/plant_comp_damping_x.pdf">pdf</a>)</p>
+<p><span class="figure-number">Figure 24: </span>Plant for the \(x\) direction for different active damping technique used (<a href="./figs/plant_comp_damping_x.png">png</a>, <a href="./figs/plant_comp_damping_x.pdf">pdf</a>)</p>
 </div>
 
 
-<div id="orgdaa95b3" class="figure">
+<div id="orge00f3b6" class="figure">
 <p><img src="figs/plant_comp_damping_coupling.png" alt="plant_comp_damping_coupling.png" />
 </p>
-<p><span class="figure-number">Figure 33: </span>Comparison of one off-diagonal plant for different damping technique applied (<a href="./figs/plant_comp_damping_coupling.png">png</a>, <a href="./figs/plant_comp_damping_coupling.pdf">pdf</a>)</p>
+<p><span class="figure-number">Figure 25: </span>Comparison of one off-diagonal plant for different damping technique applied (<a href="./figs/plant_comp_damping_coupling.png">png</a>, <a href="./figs/plant_comp_damping_coupling.pdf">pdf</a>)</p>
+</div>
+</div>
+</div>
+<div id="outline-container-orgc2cfa6b" class="outline-3">
+<h3 id="orgc2cfa6b"><span class="section-number-3">5.4</span> Tomography Experiment</h3>
+<div class="outline-text-3" id="text-5-4">
+<div class="org-src-container">
+<pre class="src src-matlab">load(<span class="org-string">'./active_damping/mat/tomo_exp.mat'</span>, <span class="org-string">'En'</span>, <span class="org-string">'En_iff'</span>, <span class="org-string">'En_dvf'</span>, <span class="org-string">'En_ine'</span>);
+t = linspace(0, 3, length(En(<span class="org-type">:</span>,1)));
+</pre>
+</div>
+
+<div class="org-src-container">
+<pre class="src src-matlab">rms(sqrt(En(<span class="org-type">:</span>, 1)<span class="org-type">.^</span>2     <span class="org-type">+</span> En(<span class="org-type">:</span>, 2)<span class="org-type">.^</span>2     <span class="org-type">+</span> En(<span class="org-type">:</span>, 3)<span class="org-type">.^</span>2))
+rms(sqrt(En_ine(<span class="org-type">:</span>, 1)<span class="org-type">.^</span>2 <span class="org-type">+</span> En_ine(<span class="org-type">:</span>, 2)<span class="org-type">.^</span>2 <span class="org-type">+</span> En_ine(<span class="org-type">:</span>, 3)<span class="org-type">.^</span>2))
+rms(sqrt(En_dvf(<span class="org-type">:</span>, 1)<span class="org-type">.^</span>2 <span class="org-type">+</span> En_dvf(<span class="org-type">:</span>, 2)<span class="org-type">.^</span>2 <span class="org-type">+</span> En_dvf(<span class="org-type">:</span>, 3)<span class="org-type">.^</span>2))
+rms(sqrt(En_iff(<span class="org-type">:</span>, 1)<span class="org-type">.^</span>2 <span class="org-type">+</span> En_iff(<span class="org-type">:</span>, 2)<span class="org-type">.^</span>2 <span class="org-type">+</span> En_iff(<span class="org-type">:</span>, 3)<span class="org-type">.^</span>2))
+</pre>
+</div>
+</div>
+
+<div id="outline-container-orgb357d35" class="outline-4">
+<h4 id="orgb357d35"><span class="section-number-4">5.4.1</span> Frequency Domain</h4>
+<div class="outline-text-4" id="text-5-4-1">
+<div class="org-src-container">
+<pre class="src src-matlab">Ts = t(1); <span class="org-comment">% Sample Time for the Data [s]</span>
+
+n_av = 8;
+han_win = hanning(ceil(length(En(<span class="org-type">:</span>, 1))<span class="org-type">/</span>n_av));
+
+[pdx, f] = pwelch(Ern(<span class="org-type">:</span>, 1), han_win, [], [], 1<span class="org-type">/</span>Ts);
+</pre>
+</div>
 </div>
 </div>
 </div>
 </div>
 
-<div id="outline-container-org0c54368" class="outline-2">
-<h2 id="org0c54368"><span class="section-number-2">6</span> Conclusion</h2>
+<div id="outline-container-orgb95a1fb" class="outline-2">
+<h2 id="orgb95a1fb"><span class="section-number-2">6</span> Useful Functions</h2>
 <div class="outline-text-2" id="text-6">
+</div>
+<div id="outline-container-org624fc0d" class="outline-3">
+<h3 id="org624fc0d"><span class="section-number-3">6.1</span> prepareTomographyExperiment</h3>
+<div class="outline-text-3" id="text-6-1">
 <p>
-<a id="org42c2dd7"></a>
+<a id="org2f22626"></a>
 </p>
+
+<p>
+This Matlab function is accessible <a href="src/prepareTomographyExperiment.m">here</a>.
+</p>
+</div>
+
+<div id="outline-container-org4917bc7" class="outline-4">
+<h4 id="org4917bc7"><span class="section-number-4">6.1.1</span> Function Description</h4>
+<div class="outline-text-4" id="text-6-1-1">
+<div class="org-src-container">
+<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[]</span> = <span class="org-function-name">prepareTomographyExperiment</span>(<span class="org-variable-name">args</span>)
+</pre>
+</div>
+</div>
+</div>
+
+<div id="outline-container-org3c7b365" class="outline-4">
+<h4 id="org3c7b365"><span class="section-number-4">6.1.2</span> Optional Parameters</h4>
+<div class="outline-text-4" id="text-6-1-2">
+<div class="org-src-container">
+<pre class="src src-matlab">arguments
+    args.nass_actuator       char   {mustBeMember(args.nass_actuator,{<span class="org-string">'piezo'</span>, <span class="org-string">'lorentz'</span>})} = <span class="org-string">'piezo'</span>
+    args.sample_mass   (1,1) double {mustBeNumeric, mustBePositive} = 50
+    args.Ry_period     (1,1) double {mustBeNumeric, mustBePositive} = 1
+<span class="org-keyword">end</span>
+</pre>
+</div>
+</div>
+</div>
+
+<div id="outline-container-org758e52a" class="outline-4">
+<h4 id="org758e52a"><span class="section-number-4">6.1.3</span> Initialize the Simulation</h4>
+<div class="outline-text-4" id="text-6-1-3">
+<p>
+We initialize all the stages with the default parameters.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+initializeAxisc();
+initializeMirror();
+</pre>
+</div>
+
+<p>
+The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">initializeNanoHexapod(<span class="org-string">'actuator'</span>, args.nass_actuator);
+initializeSample(<span class="org-string">'mass'</span>, args.sample_mass);
+</pre>
+</div>
+
+<p>
+We set the references to zero.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">initializeReferences(<span class="org-string">'Rz_type'</span>, <span class="org-string">'rotating'</span>, <span class="org-string">'Rz_period'</span>, args.Ry_period);
+</pre>
+</div>
+
+<p>
+And all the controllers are set to 0.
+</p>
+<div class="org-src-container">
+<pre class="src src-matlab">K = tf(zeros(6));
+save(<span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K'</span>, <span class="org-string">'-append'</span>);
+K_ine = tf(zeros(6));
+save(<span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_ine'</span>, <span class="org-string">'-append'</span>);
+K_iff = tf(zeros(6));
+save(<span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_iff'</span>, <span class="org-string">'-append'</span>);
+K_dvf = tf(zeros(6));
+save(<span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_dvf'</span>, <span class="org-string">'-append'</span>);
+</pre>
+</div>
+</div>
+</div>
 </div>
 </div>
 </div>
 <div id="postamble" class="status">
 <p class="author">Author: Dehaeze Thomas</p>
-<p class="date">Created: 2019-12-11 mer. 16:55</p>
-<p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p>
+<p class="date">Created: 2020-01-15 mer. 16:22</p>
 </div>
 </body>
 </html>
diff --git a/active_damping/index.org b/active_damping/index.org
index 0c27dd1..c027782 100644
--- a/active_damping/index.org
+++ b/active_damping/index.org
@@ -1,4 +1,4 @@
-#+TITLE: Active Damping
+#+TITLE: Active Damping applied on the Simscape Model
 :DRAWER:
 #+STARTUP: overview
 
@@ -46,16 +46,16 @@ First, in section [[sec:undamped_system]], we will looked at the undamped system
 
 Then, we will compare three active damping techniques:
 - In section [[sec:iff]]: the integral force feedback is used
-- In section [[sec:rmc]]: the relative motion control is used
 - In section [[sec:dvf]]: the direct velocity feedback is used
+- In section [[sec:ine]]: inertial control is used
 
 For each of the active damping technique, we will:
-- Compare the sensitivity from disturbances
 - Look at the damped plant
+- Simulate tomography experiments
+- Compare the sensitivity from disturbances
 
 The disturbances are:
 - Ground motion
-- Direct forces
 - Motion errors of all the stages
 
 * Undamped System
@@ -79,11 +79,11 @@ The disturbances are:
   All the files (data and Matlab scripts) are accessible [[file:data/undamped_system.zip][here]].
 #+end_note
 
-** Introduction                                                     :ignore:
+** Introduction                                                      :ignore:
 We first look at the undamped system.
 The performance of this undamped system will be compared with the damped system using various techniques.
 
-** Matlab Init                                                :noexport:ignore:
+** 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
@@ -96,16 +96,18 @@ The performance of this undamped system will be compared with the damped system
   simulinkproject('../');
 #+end_src
 
+#+begin_src matlab
+  addpath('active_damping/src/');
+#+end_src
+
 #+begin_src matlab
   open('active_damping/matlab/sim_nass_active_damping.slx')
 #+end_src
 
-** Init
+** Identification of the dynamics for Active Damping
+*** Initialize the Simulation
 We initialize all the stages with the default parameters.
-The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
-
 #+begin_src matlab
-  initializeReferences();
   initializeGround();
   initializeGranite();
   initializeTy();
@@ -114,21 +116,33 @@ The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
   initializeMicroHexapod();
   initializeAxisc();
   initializeMirror();
-  initializeNanoHexapod('actuator', 'piezo');
-  initializeSample('mass', 1);
 #+end_src
 
-All the controllers are set to 0.
+The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
+#+begin_src matlab
+  initializeNanoHexapod('actuator', 'piezo');
+  initializeSample('mass', 50);
+#+end_src
+
+We set the references to zero.
+#+begin_src matlab
+  initializeReferences();
+#+end_src
+
+And all the controllers are set to 0.
 #+begin_src matlab
   K = tf(zeros(6));
   save('./mat/controllers.mat', 'K', '-append');
+  K_ine = tf(zeros(6));
+  save('./mat/controllers.mat', 'K_ine', '-append');
   K_iff = tf(zeros(6));
   save('./mat/controllers.mat', 'K_iff', '-append');
   K_dvf = tf(zeros(6));
   save('./mat/controllers.mat', 'K_dvf', '-append');
 #+end_src
 
-** Identification of the transfer function from disturbance to position error
+*** Identification
+First, we identify the dynamics of the system using the =linearize= function.
 #+begin_src matlab
   %% Options for Linearized
   options = linearizeOptions;
@@ -139,314 +153,32 @@ All the controllers are set to 0.
 
   %% Input/Output definition
   clear io; io_i = 1;
-  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwx'); io_i = io_i + 1;
-  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwy'); io_i = io_i + 1;
-  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz'); io_i = io_i + 1;
-  io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1;
-
-  %% Run the linearization
-  G = linearize(mdl, io, options);
-  G.InputName  = {'Dwx', 'Dwy', 'Dwz'};
-  G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
-#+end_src
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G('Edx', 'Dwx'), freqs, 'Hz'))), 'DisplayName', 'x');
-  plot(freqs, abs(squeeze(freqresp(G('Edy', 'Dwz'), freqs, 'Hz'))), 'DisplayName', 'y');
-  plot(freqs, abs(squeeze(freqresp(G('Edz', 'Dwz'), freqs, 'Hz'))), 'DisplayName', 'z');
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
-  legend('location', 'southeast')
-#+end_src
-
-** Identification of the plant
-#+begin_src matlab
-  %% Options for Linearized
-  options = linearizeOptions;
-  options.SampleTime = 0;
-
-  %% Name of the Simulink File
-  mdl = 'sim_nass_active_damping';
-
-  %% Input/Output definition
-  clear io; io_i = 1;
-  io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
-  io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1;
-
-  %% Run the linearization
-  G = linearize(mdl, io, options);
-  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
-  G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
-#+end_src
-
-#+begin_src matlab
-  load('mat/stages.mat', 'nano_hexapod');
-  G_cart = G*inv(nano_hexapod.J');
-  G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
-#+end_src
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  ax1 = subplot(2, 1, 1);
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G_cart('Edx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$T_{x}$');
-  plot(freqs, abs(squeeze(freqresp(G_cart('Edy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$T_{y}$');
-  plot(freqs, abs(squeeze(freqresp(G_cart('Edz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$T_{z}$');
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-  legend('location', 'southwest')
-
-  ax2 = subplot(2, 1, 2);
-  hold on;
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Edx', 'Fnx'), freqs, 'Hz'))));
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Edy', 'Fny'), freqs, 'Hz'))));
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Edz', 'Fnz'), freqs, 'Hz'))));
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-
-  linkaxes([ax1,ax2],'x');
-#+end_src
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  ax1 = subplot(2, 1, 1);
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$R_{x}$');
-  plot(freqs, abs(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$R_{y}$');
-  plot(freqs, abs(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$R_{z}$');
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-  legend('location', 'southwest')
-
-  ax2 = subplot(2, 1, 2);
-  hold on;
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))));
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))));
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))));
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-
-  linkaxes([ax1,ax2],'x');
-#+end_src
-
-** test
-#+begin_src matlab
-  %% Options for Linearized
-  options = linearizeOptions;
-  options.SampleTime = 0;
-
-  %% Name of the Simulink File
-  mdl = 'sim_nass_active_damping';
-
-  %% Input/Output definition
-  clear io; io_i = 1;
-  io(io_i) = linio([mdl, '/Micro-Station/Dy'], 1, 'openinput'); io_i = io_i + 1;
-  io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1;
-
-  %% Run the linearization
-  G = linearize(mdl, io, options);
-  G.InputName  = {'Dy'};
-  G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
-#+end_src
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  ax1 = subplot(2, 1, 1);
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))), 'DisplayName', '$T_{x}$');
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-  legend('location', 'southwest')
-
-  ax2 = subplot(2, 1, 2);
-  hold on;
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))));
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-
-  linkaxes([ax1,ax2],'x');
-#+end_src
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  ax1 = subplot(2, 1, 1);
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$R_{x}$');
-  plot(freqs, abs(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$R_{y}$');
-  plot(freqs, abs(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$R_{z}$');
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-  legend('location', 'southwest')
-
-  ax2 = subplot(2, 1, 2);
-  hold on;
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))));
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))));
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))));
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-
-  linkaxes([ax1,ax2],'x');
-#+end_src
-
-** test on hexapod
-#+begin_src matlab
-  %% Options for Linearized
-  options = linearizeOptions;
-  options.SampleTime = 0;
-
-  %% Name of the Simulink File
-  mdl = 'test_nano_hexapod';
-
-  %% Input/Output definition
-  clear io; io_i = 1;
-  io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
-  io(io_i) = linio([mdl, '/x'], 1, 'openoutput'); io_i = io_i + 1;
-  io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;
-  io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;
-
-  %% Run the linearization
-  G = linearize(mdl, io, options);
-  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
-  G.OutputName = {'x', 'y', 'z'};
-#+end_src
-
-#+begin_src matlab
-  %% Options for Linearized
-  options = linearizeOptions;
-  options.SampleTime = 0;
-
-  %% Name of the Simulink File
-  mdl = 'test_nano_hexapod';
-
-  %% Input/Output definition
-  clear io; io_i = 1;
-  io(io_i) = linio([mdl, '/Fx'], 1, 'openinput'); io_i = io_i + 1;
-  io(io_i) = linio([mdl, '/x'], 1, 'openoutput'); io_i = io_i + 1;
-  io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;
-  io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;
-
-  %% Run the linearization
-  G = linearize(mdl, io, options);
-  G.InputName  = {'Fx'};
-  G.OutputName = {'x', 'y', 'z'};
-#+end_src
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  ax1 = subplot(2, 1, 1);
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))), 'DisplayName', '$T_{x}$');
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-  legend('location', 'southwest')
-
-  ax2 = subplot(2, 1, 2);
-  hold on;
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))));
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-
-  linkaxes([ax1,ax2],'x');
-#+end_src
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  ax1 = subplot(2, 1, 1);
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$R_{x}$');
-  plot(freqs, abs(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$R_{y}$');
-  plot(freqs, abs(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$R_{z}$');
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-  legend('location', 'southwest')
-
-  ax2 = subplot(2, 1, 2);
-  hold on;
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))));
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))));
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))));
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-
-  linkaxes([ax1,ax2],'x');
-#+end_src
-
-** Identification of the dynamics for Active Damping
-#+begin_src matlab
-  %% Options for Linearized
-  options = linearizeOptions;
-  options.SampleTime = 0;
-
-  %% Name of the Simulink File
-  mdl = 'sim_nass_active_damping';
-
-  %% Input/Output definition
-  clear io; io_i = 1;
-  io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Fnl'],           1, 'openinput');              io_i = io_i + 1;
   io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
   io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vlm');  io_i = io_i + 1;
 
   %% Run the linearization
   G = linearize(mdl, io, options);
   G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
   G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
-                  'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
+                  'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
+                  'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'};
 #+end_src
 
-And we save it for further analysis.
+We then create transfer functions corresponding to the active damping plants.
 #+begin_src matlab
-  save('./active_damping/mat/plants.mat', 'G', '-append');
+  G_iff = minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
+  G_dvf = minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
+  G_ine = minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
 #+end_src
 
-** Plant for Active Damping
+And we save them for further analysis.
+#+begin_src matlab
+  save('./active_damping/mat/undamped_plants.mat', 'G_iff', 'G_dvf', 'G_ine');
+#+end_src
+
+*** Obtained Plants for Active Damping
 #+begin_src matlab :exports none
   freqs = logspace(0, 3, 1000);
 
@@ -455,7 +187,7 @@ And we save it for further analysis.
   ax1 = subplot(2, 1, 1);
   hold on;
   for i = 1:6
-    plot(freqs, abs(squeeze(freqresp(G(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+    plot(freqs, abs(squeeze(freqresp(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -464,7 +196,7 @@ And we save it for further analysis.
   ax2 = subplot(2, 1, 2);
   hold on;
   for i = 1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
@@ -475,6 +207,15 @@ And we save it for further analysis.
   linkaxes([ax1,ax2],'x');
 #+end_src
 
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/nass_active_damping_iff_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+NAME: fig:nass_active_damping_iff_plant
+#+CAPTION: =G_iff=: IFF Plant ([[./figs/nass_active_damping_iff_plant.png][png]], [[./figs/nass_active_damping_iff_plant.pdf][pdf]])
+[[file:figs/nass_active_damping_iff_plant.png]]
+
 #+begin_src matlab :exports none
   freqs = logspace(0, 3, 1000);
 
@@ -483,7 +224,7 @@ And we save it for further analysis.
   ax1 = subplot(2, 1, 1);
   hold on;
   for i = 1:6
-    plot(freqs, abs(squeeze(freqresp(G(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+    plot(freqs, abs(squeeze(freqresp(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -492,7 +233,7 @@ And we save it for further analysis.
   ax2 = subplot(2, 1, 2);
   hold on;
   for i = 1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+    plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
@@ -503,68 +244,14 @@ And we save it for further analysis.
   linkaxes([ax1,ax2],'x');
 #+end_src
 
-** Sensitivity to disturbances
-The sensitivity to disturbances are shown on figure [[fig:sensitivity_dist_undamped]].
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  subplot(2, 1, 1);
-  title('$D_g$ to $D$');
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
-  legend('location', 'southeast');
-
-  subplot(2, 1, 2);
-  title('$F_s$ to $D$');
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
-  legend('location', 'northeast');
-#+end_src
-
 #+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/sensitivity_dist_undamped.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
+#+begin_src matlab :var filepath="figs/nass_active_damping_dvf_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
 #+end_src
 
-#+NAME: fig:sensitivity_dist_undamped
-#+CAPTION: Undamped sensitivity to disturbances ([[./figs/sensitivity_dist_undamped.png][png]], [[./figs/sensitivity_dist_undamped.pdf][pdf]])
-[[file:figs/sensitivity_dist_undamped.png]]
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
-  legend('location', 'northeast');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/sensitivity_dist_stages.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:sensitivity_dist_stages
-#+CAPTION: Sensitivity to force disturbances in various stages ([[./figs/sensitivity_dist_stages.png][png]], [[./figs/sensitivity_dist_stages.pdf][pdf]])
-[[file:figs/sensitivity_dist_stages.png]]
-
-** Undamped Plant
-The "plant" (transfer function from forces applied by the nano-hexapod to the measured displacement of the sample with respect to the granite) bode plot is shown on figure [[fig:sensitivity_dist_undamped]].
+#+NAME: fig:nass_active_damping_dvf_plant
+#+CAPTION: =G_dvf=: Plant for Direct Velocity Feedback ([[./figs/nass_active_damping_dvf_plant.png][png]], [[./figs/nass_active_damping_dvf_plant.pdf][pdf]])
+[[file:figs/nass_active_damping_ine_plant.png]]
 
 #+begin_src matlab :exports none
   freqs = logspace(0, 3, 1000);
@@ -573,38 +260,107 @@ The "plant" (transfer function from forces applied by the nano-hexapod to the me
 
   ax1 = subplot(2, 1, 1);
   hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
-  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
-  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
+  for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+  end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  ylabel('Amplitude [(m/s)/N]'); set(gca, 'XTickLabel',[]);
 
   ax2 = subplot(2, 1, 2);
   hold on;
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$D_x / F_x$');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$D_y / F_y$');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$D_z / F_z$');
+  for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+  end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
   ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
   ylim([-180, 180]);
   yticks([-180, -90, 0, 90, 180]);
-  legend('location', 'southwest');
 
   linkaxes([ax1,ax2],'x');
 #+end_src
 
 #+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/plant_undamped.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
+#+begin_src matlab :var filepath="figs/nass_active_damping_inertial_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
 #+end_src
 
-#+NAME: fig:plant_undamped
-#+CAPTION: Transfer Function from cartesian forces to displacement for the undamped plant ([[./figs/plant_undamped.png][png]], [[./figs/plant_undamped.pdf][pdf]])
-[[file:figs/plant_undamped.png]]
+#+NAME: fig:nass_active_damping_inertial_plant
+#+CAPTION: Inertial Feedback Plant ([[./figs/nass_active_damping_inertial_plant.png][png]], [[./figs/nass_active_damping_inertial_plant.pdf][pdf]])
+[[file:figs/nass_active_damping_inertial_plant.png]]
 
 ** Tomography Experiment
+*** Simulation
+We initialize elements for the tomography experiment.
+#+begin_src matlab
+  prepareTomographyExperiment();
+#+end_src
+
+We change the simulation stop time.
+#+begin_src matlab
+  load('mat/conf_simscape.mat');
+  set_param(conf_simscape, 'StopTime', '3');
+#+end_src
+
+And we simulate the system.
+#+begin_src matlab
+  sim('sim_nass_active_damping');
+#+end_src
+
+Finally, we save the simulation results for further analysis
+#+begin_src matlab
+  save('./active_damping/mat/tomo_exp.mat', 'En', 'Eg', '-append');
+#+end_src
+
+*** Results
+We load the results of tomography experiments.
+#+begin_src matlab
+  load('./active_damping/mat/tomo_exp.mat', 'En');
+  t = linspace(0, 3, length(En(:,1)));
+#+end_src
+
+#+begin_src matlab :exports none
+  figure;
+  hold on;
+  plot(t, En(:,1), 'DisplayName', '$\epsilon_{x}$')
+  plot(t, En(:,2), 'DisplayName', '$\epsilon_{y}$')
+  plot(t, En(:,3), 'DisplayName', '$\epsilon_{z}$')
+  hold off;
+  legend();
+  xlabel('Time [s]'); ylabel('Position Error [m]');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/nass_act_damp_undamped_sim_tomo_trans.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+NAME: fig:nass_act_damp_undamped_sim_tomo_trans
+#+CAPTION: Position Error during tomography experiment - Translations ([[./figs/nass_act_damp_undamped_sim_tomo_trans.png][png]], [[./figs/nass_act_damp_undamped_sim_tomo_trans.pdf][pdf]])
+[[file:figs/nass_act_damp_undamped_sim_tomo_trans.png]]
+
+#+begin_src matlab :exports none
+  figure;
+  hold on;
+  plot(t, En(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
+  plot(t, En(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
+  plot(t, En(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
+  hold off;
+  xlim([0.5,inf]);
+  legend();
+  xlabel('Time [s]'); ylabel('Position Error [rad]');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/nass_act_damp_undamped_sim_tomo_rot.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+NAME: fig:nass_act_damp_undamped_sim_tomo_rot
+#+CAPTION: Position Error during tomography experiment - Rotations ([[./figs/nass_act_damp_undamped_sim_tomo_rot.png][png]], [[./figs/nass_act_damp_undamped_sim_tomo_rot.pdf][pdf]])
+[[file:figs/nass_act_damp_undamped_sim_tomo_rot.png]]
+
 * Integral Force Feedback
 :PROPERTIES:
 :header-args:matlab+: :tangle matlab/iff.m
@@ -627,12 +383,10 @@ The "plant" (transfer function from forces applied by the nano-hexapod to the me
   All the files (data and Matlab scripts) are accessible [[file:data/iff.zip][here]].
 #+end_note
 
-** Introduction                                                     :ignore:
-Integral Force Feedback is applied.
-In section [[sec:iff_1dof]], IFF is applied on a uni-axial system to understand its behavior.
-Then, it is applied on the simscape model.
+** Introduction                                                      :ignore:
+Integral Force Feedback is applied on the simscape model.
 
-** Matlab Init                                             :noexport:ignore:
+** 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
@@ -646,13 +400,18 @@ Then, it is applied on the simscape model.
 #+end_src
 
 #+begin_src matlab
-  open('active_damping/matlab/sim_nano_station_id.slx')
+  addpath('active_damping/src/');
+#+end_src
+
+#+begin_src matlab
+  open('active_damping/matlab/sim_nass_active_damping.slx')
 #+end_src
 
 ** Control Design
-Let's load the undamped plant:
+*** Plant
+Let's load the previously indentified undamped plant:
 #+begin_src matlab
-  load('./active_damping/mat/plants.mat', 'G');
+  load('./active_damping/mat/undamped_plants.mat', 'G_iff');
 #+end_src
 
 Let's look at the transfer function from actuator forces in the nano-hexapod to the force sensor in the nano-hexapod legs for all 6 pairs of actuator/sensor (figure [[fig:iff_plant]]).
@@ -665,7 +424,7 @@ Let's look at the transfer function from actuator forces in the nano-hexapod to
   ax1 = subplot(2, 1, 1);
   hold on;
   for i=1:6
-    plot(freqs, abs(squeeze(freqresp(G.G_iff(['Fm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
+    plot(freqs, abs(squeeze(freqresp(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -674,7 +433,7 @@ Let's look at the transfer function from actuator forces in the nano-hexapod to
   ax2 = subplot(2, 1, 2);
   hold on;
   for i=1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_iff(['Fm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
+    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
@@ -694,9 +453,10 @@ Let's look at the transfer function from actuator forces in the nano-hexapod to
 #+CAPTION: Transfer function from forces applied in the legs to force sensor ([[./figs/iff_plant.png][png]], [[./figs/iff_plant.pdf][pdf]])
 [[file:figs/iff_plant.png]]
 
+*** Control Design
 The controller for each pair of actuator/sensor is:
 #+begin_src matlab
-  K_iff = -1000/s;
+  K_iff = 1000/s;
 #+end_src
 
 The corresponding loop gains are shown in figure [[fig:iff_open_loop]].
@@ -709,7 +469,7 @@ The corresponding loop gains are shown in figure [[fig:iff_open_loop]].
   ax1 = subplot(2, 1, 1);
   hold on;
   for i=1:6
-    plot(freqs, abs(squeeze(freqresp(K_iff*G.G_iff(['Fm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
+    plot(freqs, abs(squeeze(freqresp(K_iff*G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -718,7 +478,7 @@ The corresponding loop gains are shown in figure [[fig:iff_open_loop]].
   ax2 = subplot(2, 1, 2);
   hold on;
   for i=1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(K_iff*G.G_iff(['Fm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
+    plot(freqs, 180/pi*angle(squeeze(freqresp(K_iff*G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
@@ -738,45 +498,90 @@ The corresponding loop gains are shown in figure [[fig:iff_open_loop]].
 #+CAPTION: Loop Gain for the Integral Force Feedback ([[./figs/iff_open_loop.png][png]], [[./figs/iff_open_loop.pdf][pdf]])
 [[file:figs/iff_open_loop.png]]
 
-** Identification of the damped plant
-Let's initialize the system prior to identification.
+*** Diagonal Controller
+We create the diagonal controller and we add a minus sign as we have a positive
+feedback architecture.
+#+begin_src matlab
+  K_iff = -K_iff*eye(6);
+#+end_src
+
+We save the controller for further analysis.
+#+begin_src matlab
+  save('./active_damping/mat/K_iff.mat', 'K_iff');
+#+end_src
+
+** TODO Identification of the damped plant                         :noexport:
+*** Initialize the Simulation
+We initialize all the stages with the default parameters.
+#+begin_src matlab
+    initializeGround();
+    initializeGranite();
+    initializeTy();
+    initializeRy();
+    initializeRz();
+    initializeMicroHexapod();
+    initializeAxisc();
+    initializeMirror();
+#+end_src
+
+The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
 #+begin_src matlab
-  initializeReferences();
-  initializeGround();
-  initializeGranite();
-  initializeTy();
-  initializeRy();
-  initializeRz();
-  initializeMicroHexapod();
-  initializeAxisc();
-  initializeMirror();
   initializeNanoHexapod('actuator', 'piezo');
   initializeSample('mass', 50);
 #+end_src
 
-All the controllers are set to 0.
+We set the references to zero.
+#+begin_src matlab
+  initializeReferences();
+#+end_src
+
+And all the controllers are set to 0 except for the IFF.
 #+begin_src matlab
   K = tf(zeros(6));
   save('./mat/controllers.mat', 'K', '-append');
-  K_iff = -K_iff*eye(6);
+  K_ine = tf(zeros(6));
+  save('./mat/controllers.mat', 'K_ine', '-append');
+  K_iff = K_iff;
   save('./mat/controllers.mat', 'K_iff', '-append');
-  K_rmc = tf(zeros(6));
-  save('./mat/controllers.mat', 'K_rmc', '-append');
   K_dvf = tf(zeros(6));
   save('./mat/controllers.mat', 'K_dvf', '-append');
 #+end_src
 
-We identify the system dynamics now that the IFF controller is ON.
+*** Identification
+First, we identify the dynamics of the system using the =linearize= function.
 #+begin_src matlab
-  G_iff = identifyPlant();
+  %% Options for Linearized
+  options = linearizeOptions;
+  options.SampleTime = 0;
+
+  %% Name of the Simulink File
+  mdl = 'sim_nass_active_damping';
+
+  %% Input/Output definition
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Fnl'],           1, 'openinput');              io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
+
+  %% Run the linearization
+  G = linearize(mdl, io, options);
+  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+  G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
+                  'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
 #+end_src
 
-And we save the damped plant for further analysis
+We then create transfer functions corresponding to the active damping plants.
+#+begin_src matlab
+  G_iff = minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
+  % G_rmc = minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
+#+end_src
+
+And we save them for further analysis.
 #+begin_src matlab
   save('./active_damping/mat/plants.mat', 'G_iff', '-append');
 #+end_src
 
-** Sensitivity to disturbances
+*** TODO Sensitivity to disturbances
 As shown on figure [[fig:sensitivity_dist_iff]]:
 - The top platform of the nano-hexapod how behaves as a "free-mass".
 - The transfer function from direct forces $F_s$ to the relative displacement $D$ is equivalent to the one of an isolated mass.
@@ -857,7 +662,7 @@ As shown on figure [[fig:sensitivity_dist_iff]]:
 #+CAPTION: Sensitivity to force disturbances in various stages when IFF is applied ([[./figs/sensitivity_dist_stages_iff.png][png]], [[./figs/sensitivity_dist_stages_iff.pdf][pdf]])
 [[file:figs/sensitivity_dist_stages_iff.png]]
 
-** Damped Plant
+*** TODO Damped Plant
 Now, look at the new damped plant to control.
 
 It damps the plant (resonance of the nano hexapod as well as other resonances) as shown in figure [[fig:plant_iff_damped]].
@@ -962,6 +767,97 @@ However, it increases coupling at low frequency (figure [[fig:plant_iff_coupling
 #+CAPTION: Coupling induced by IFF ([[./figs/plant_iff_coupling.png][png]], [[./figs/plant_iff_coupling.pdf][pdf]])
 [[file:figs/plant_iff_coupling.png]]
 
+** Tomography Experiment
+*** Initialize the Simulation
+We initialize elements for the tomography experiment.
+#+begin_src matlab
+  prepareTomographyExperiment();
+#+end_src
+
+We set the IFF controller.
+#+begin_src matlab
+  load('./active_damping/mat/K_iff.mat', 'K_iff');
+  save('./mat/controllers.mat', 'K_iff', '-append');
+#+end_src
+
+*** Simulation
+We change the simulation stop time.
+#+begin_src matlab
+  load('mat/conf_simscape.mat');
+  set_param(conf_simscape, 'StopTime', '3');
+#+end_src
+
+And we simulate the system.
+#+begin_src matlab
+  sim('sim_nass_active_damping');
+#+end_src
+
+Finally, we save the simulation results for further analysis
+#+begin_src matlab
+  En_iff = En;
+  Eg_iff = Eg;
+  save('./active_damping/mat/tomo_exp.mat', 'En_iff', 'Eg_iff', '-append');
+#+end_src
+
+*** Compare with Undamped system
+We load the results of tomography experiments.
+#+begin_src matlab
+  load('./active_damping/mat/tomo_exp.mat', 'En', 'En_iff');
+#+end_src
+
+#+begin_src matlab
+  t = linspace(0, 3, length(En(:,1)));
+#+end_src
+
+#+begin_src matlab :exports none
+  figure;
+  hold on;
+  plot(t, En(:,1), 'DisplayName', '$\epsilon_{x}$')
+  plot(t, En(:,2), 'DisplayName', '$\epsilon_{y}$')
+  plot(t, En(:,3), 'DisplayName', '$\epsilon_{z}$')
+  set(gca,'ColorOrderIndex',1);
+  plot(t, En_iff(:,1), '--', 'DisplayName', '$\epsilon_{x}$ - IFF')
+  plot(t, En_iff(:,2), '--', 'DisplayName', '$\epsilon_{y}$ - IFF')
+  plot(t, En_iff(:,3), '--', 'DisplayName', '$\epsilon_{z}$ - IFF')
+  hold off;
+  legend('location', 'northwest');
+  xlabel('Time [s]'); ylabel('Position Error [m]');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/nass_act_damp_iff_sim_tomo_trans.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+NAME: fig:nass_act_damp_iff_sim_tomo_trans
+#+CAPTION: Position Error during tomography experiment - Translations ([[./figs/nass_act_damp_iff_sim_tomo_trans.png][png]], [[./figs/nass_act_damp_iff_sim_tomo_trans.pdf][pdf]])
+[[file:figs/nass_act_damp_iff_sim_tomo_trans.png]]
+
+#+begin_src matlab :exports none
+  figure;
+  hold on;
+  plot(t, En(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
+  plot(t, En(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
+  plot(t, En(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
+  set(gca,'ColorOrderIndex',1);
+  plot(t, En_iff(:,4), '--', 'DisplayName', '$\epsilon_{\theta_x}$ - IFF')
+  plot(t, En_iff(:,5), '--', 'DisplayName', '$\epsilon_{\theta_y}$ - IFF')
+  plot(t, En_iff(:,6), '--', 'DisplayName', '$\epsilon_{\theta_z}$ - IFF')
+  hold off;
+  xlim([0.5,inf]);
+  legend();
+  xlabel('Time [s]'); ylabel('Position Error [rad]');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/nass_act_damp_iff_sim_tomo_rot.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+NAME: fig:nass_act_damp_iff_sim_tomo_rot
+#+CAPTION: Position Error during tomography experiment - Rotations ([[./figs/nass_act_damp_iff_sim_tomo_rot.png][png]], [[./figs/nass_act_damp_iff_sim_tomo_rot.pdf][pdf]])
+[[file:figs/nass_act_damp_iff_sim_tomo_rot.png]]
+
 ** Conclusion
 #+begin_important
 Integral Force Feedback:
@@ -970,37 +866,1589 @@ Integral Force Feedback:
 - Increase the sensitivity to disturbances at low frequencies
 #+end_important
 
-* Relative Motion Control
+* Direct Velocity Feedback
 :PROPERTIES:
-:header-args:matlab+: :tangle matlab/rmc.m
+:header-args:matlab+: :tangle matlab/dvf.m
 :header-args:matlab+: :comments none :mkdirp yes
 :END:
-<<sec:rmc>>
+<<sec:dvf>>
 
 ** ZIP file containing the data and matlab files                     :ignore:
 #+begin_src bash :exports none :results none
-  if [ matlab/rmc.m -nt data/rmc.zip ]; then
-    cp matlab/rmc.m rmc.m;
-    zip data/rmc \
+  if [ matlab/dvf.m -nt data/dvf.zip ]; then
+    cp matlab/dvf.m dvf.m;
+    zip data/dvf \
         mat/plant.mat \
-        rmc.m
-    rm rmc.m;
+        dvf.m
+    rm dvf.m;
   fi
 #+end_src
 
 #+begin_note
-  All the files (data and Matlab scripts) are accessible [[file:data/rmc.zip][here]].
+  All the files (data and Matlab scripts) are accessible [[file:data/dvf.zip][here]].
 #+end_note
 
-** Introduction                                                     :ignore:
-In the Relative Motion Control (RMC), a derivative feedback is applied between the measured actuator displacement to the actuator force input.
+** Introduction                                                      :ignore:
+In the Direct Velocity Feedback (DVF), a derivative feedback is applied between the measured actuator displacement to the actuator force input.
+The actuator displacement can be measured with a capacitive sensor for instance.
 
-** One degree-of-freedom example
+** 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
+  simulinkproject('../');
+#+end_src
+
+#+begin_src matlab
+  addpath('active_damping/src/');
+#+end_src
+
+#+begin_src matlab
+  open('active_damping/matlab/sim_nass_active_damping.slx')
+#+end_src
+
+** Control Design
+*** Plant
+Let's load the undamped plant:
+#+begin_src matlab
+  load('./active_damping/mat/undamped_plants.mat', 'G_dvf');
+#+end_src
+
+Let's look at the transfer function from actuator forces in the nano-hexapod to the measured displacement of the actuator for all 6 pairs of actuator/sensor (figure [[fig:dvf_plant]]).
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i=1:6
+    plot(freqs, abs(squeeze(freqresp(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i=1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/dvf_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:dvf_plant
+#+CAPTION: Transfer function from forces applied in the legs to leg displacement sensor ([[./figs/dvf_plant.png][png]], [[./figs/dvf_plant.pdf][pdf]])
+[[file:figs/dvf_plant.png]]
+
+*** Control Design
+The Direct Velocity Feedback is defined below.
+A Low pass Filter is added to make the controller transfer function proper.
+#+begin_src matlab
+  K_dvf = s*20000/(1 + s/2/pi/10000);
+#+end_src
+
+The obtained loop gains are shown in figure [[fig:dvf_open_loop]].
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i=1:6
+    plot(freqs, abs(squeeze(freqresp(K_dvf*G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i=1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(K_dvf*G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/dvf_open_loop.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:dvf_open_loop
+#+CAPTION: Loop Gain for the Integral Force Feedback ([[./figs/dvf_open_loop.png][png]], [[./figs/dvf_open_loop.pdf][pdf]])
+[[file:figs/dvf_open_loop.png]]
+
+*** Diagonal Controller
+We create the diagonal controller and we add a minus sign as we have a positive feedback architecture.
+#+begin_src matlab
+  K_dvf = -K_dvf*eye(6);
+#+end_src
+
+We save the controller for further analysis.
+#+begin_src matlab
+  save('./active_damping/mat/K_dvf.mat', 'K_dvf');
+#+end_src
+
+** TODO Identification of the damped plant                         :noexport:
+*** Initialize the Simulation
+Let's initialize the system prior to identification.
+#+begin_src matlab
+  initializeReferences();
+  initializeGround();
+  initializeGranite();
+  initializeTy();
+  initializeRy();
+  initializeRz();
+  initializeMicroHexapod();
+  initializeAxisc();
+  initializeMirror();
+  initializeNanoHexapod('actuator', 'piezo');
+  initializeSample('mass', 50);
+#+end_src
+
+And initialize the controllers.
+#+begin_src matlab
+  K = tf(zeros(6));
+  K_ine = tf(zeros(6));
+  save('./mat/controllers.mat', 'K_ine', '-append');
+  save('./mat/controllers.mat', 'K', '-append');
+  K_iff = tf(zeros(6));
+  save('./mat/controllers.mat', 'K_iff', '-append');
+  K_dvf = K_dvf;
+  save('./mat/controllers.mat', 'K_dvf', '-append');
+#+end_src
+
+*** Identification
+We identify the system dynamics now that the DVF controller is ON.
+#+begin_src matlab
+  %% Options for Linearized
+  options = linearizeOptions;
+  options.SampleTime = 0;
+
+  %% Name of the Simulink File
+  mdl = 'sim_nass_active_damping';
+
+  %% Input/Output definition
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
+
+  %% Run the linearization
+  G = linearize(mdl, io, options);
+  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+  G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
+                  'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
+#+end_src
+
+And we save the damped plant for further analysis.
+#+begin_src matlab
+  save('./active_damping/mat/plants.mat', 'G_dvf', '-append');
+#+end_src
+
+*** Sensitivity to disturbances
+As shown in figure [[fig:sensitivity_dist_dvf]], DVF control succeed in lowering the sensitivity to disturbances near resonance of the system.
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  subplot(2, 1, 1);
+  title('$D_g$ to $D$');
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
+  legend('location', 'southeast');
+
+  subplot(2, 1, 2);
+  title('$F_s$ to $D$');
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+  legend('location', 'northeast');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/sensitivity_dist_dvf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:sensitivity_dist_dvf
+#+CAPTION: Sensitivity to disturbance once the DVF controller is applied to the system ([[./figs/sensitivity_dist_dvf.png][png]], [[./figs/sensitivity_dist_dvf.pdf][pdf]])
+[[file:figs/sensitivity_dist_dvf.png]]
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+  legend('location', 'northeast');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/sensitivity_dist_stages_dvf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:sensitivity_dist_stages_dvf
+#+CAPTION: Sensitivity to force disturbances in various stages when DVF is applied ([[./figs/sensitivity_dist_stages_dvf.png][png]], [[./figs/sensitivity_dist_stages_dvf.pdf][pdf]])
+[[file:figs/sensitivity_dist_stages_dvf.png]]
+
+*** Damped Plant
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 2, 1);
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
+  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
+  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+
+  ax2 = subplot(2, 2, 2);
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
+  plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
+  plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
+
+  ax3 = subplot(2, 2, 3);
+  hold on;
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northwest');
+
+  ax4 = subplot(2, 2, 4);
+  hold on;
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northwest');
+
+  linkaxes([ax1,ax2,ax3,ax4],'x');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/plant_dvf_damped.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:plant_dvf_damped
+#+CAPTION: Damped Plant after DVF is applied ([[./figs/plant_dvf_damped.png][png]], [[./figs/plant_dvf_damped.pdf][pdf]])
+[[file:figs/plant_dvf_damped.png]]
+
+** Tomography Experiment
+*** Initialize the Simulation
+We initialize elements for the tomography experiment.
+#+begin_src matlab
+  prepareTomographyExperiment();
+#+end_src
+
+We set the DVF controller.
+#+begin_src matlab
+  load('./active_damping/mat/K_dvf.mat', 'K_dvf');
+  save('./mat/controllers.mat', 'K_dvf', '-append');
+#+end_src
+
+*** Simulation
+We change the simulation stop time.
+#+begin_src matlab
+  load('mat/conf_simscape.mat');
+  set_param(conf_simscape, 'StopTime', '3');
+#+end_src
+
+And we simulate the system.
+#+begin_src matlab
+  sim('sim_nass_active_damping');
+#+end_src
+
+Finally, we save the simulation results for further analysis
+#+begin_src matlab
+  En_dvf = En;
+  Eg_dvf = Eg;
+  save('./active_damping/mat/tomo_exp.mat', 'En_dvf', 'Eg_dvf', '-append');
+#+end_src
+
+*** Compare with Undamped system
+We load the results of tomography experiments.
+#+begin_src matlab
+  load('./active_damping/mat/tomo_exp.mat', 'En', 'En_dvf');
+  t = linspace(0, 3, length(En(:,1)));
+#+end_src
+
+#+begin_src matlab :exports none
+  figure;
+  hold on;
+  plot(t, En(:,1), 'DisplayName', '$\epsilon_{x}$')
+  plot(t, En(:,2), 'DisplayName', '$\epsilon_{y}$')
+  plot(t, En(:,3), 'DisplayName', '$\epsilon_{z}$')
+  set(gca,'ColorOrderIndex',1);
+  plot(t, En_dvf(:,1), '--', 'DisplayName', '$\epsilon_{x}$ - DVF')
+  plot(t, En_dvf(:,2), '--', 'DisplayName', '$\epsilon_{y}$ - DVF')
+  plot(t, En_dvf(:,3), '--', 'DisplayName', '$\epsilon_{z}$ - DVF')
+  hold off;
+  legend();
+  xlabel('Time [s]'); ylabel('Position Error [m]');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/nass_act_damp_dvf_sim_tomo_trans.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+NAME: fig:nass_act_damp_dvf_sim_tomo_trans
+#+CAPTION: Position Error during tomography experiment - Translations ([[./figs/nass_act_damp_dvf_sim_tomo_trans.png][png]], [[./figs/nass_act_damp_dvf_sim_tomo_trans.pdf][pdf]])
+[[file:figs/nass_act_damp_dvf_sim_tomo_trans.png]]
+
+#+begin_src matlab :exports none
+  figure;
+  hold on;
+  plot(t, En(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
+  plot(t, En(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
+  plot(t, En(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
+  set(gca,'ColorOrderIndex',1);
+  plot(t, En_dvf(:,4), '--', 'DisplayName', '$\epsilon_{\theta_x}$ - DVF')
+  plot(t, En_dvf(:,5), '--', 'DisplayName', '$\epsilon_{\theta_y}$ - DVF')
+  plot(t, En_dvf(:,6), '--', 'DisplayName', '$\epsilon_{\theta_z}$ - DVF')
+  hold off;
+  xlim([0.5,inf]);
+  legend();
+  xlabel('Time [s]'); ylabel('Position Error [rad]');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/nass_act_damp_dvf_sim_tomo_rot.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+NAME: fig:nass_act_damp_dvf_sim_tomo_rot
+#+CAPTION: Position Error during tomography experiment - Rotations ([[./figs/nass_act_damp_dvf_sim_tomo_rot.png][png]], [[./figs/nass_act_damp_dvf_sim_tomo_rot.pdf][pdf]])
+[[file:figs/nass_act_damp_dvf_sim_tomo_rot.png]]
+
+** Conclusion
+#+begin_important
+Direct Velocity Feedback:
+-
+#+end_important
+
+* Inertial Control
+:PROPERTIES:
+:header-args:matlab+: :tangle matlab/ine.m
+:header-args:matlab+: :comments none :mkdirp yes
+:END:
+<<sec:ine>>
+
+** ZIP file containing the data and matlab files                     :ignore:
+#+begin_src bash :exports none :results none
+  if [ matlab/ine.m -nt data/ine.zip ]; then
+    cp matlab/ine.m ine.m;
+    zip data/ine \
+        mat/plant.mat \
+        ine.m
+    rm ine.m;
+  fi
+#+end_src
+
+#+begin_note
+  All the files (data and Matlab scripts) are accessible [[file:data/ine.zip][here]].
+#+end_note
+
+** Introduction                                                      :ignore:
+In Inertial Control, a feedback is applied between the measured *absolute* velocity of the platform to the actuator force input.
+
+** 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
+  simulinkproject('../');
+#+end_src
+
+#+begin_src matlab
+  addpath('active_damping/src/');
+#+end_src
+
+#+begin_src matlab
+  open('active_damping/matlab/sim_nass_active_damping.slx')
+#+end_src
+
+** Control Design
+*** Plant
+Let's load the undamped plant:
+#+begin_src matlab
+  load('./active_damping/mat/undamped_plants.mat', 'G_ine');
+#+end_src
+
+Let's look at the transfer function from actuator forces in the nano-hexapod to the measured velocity of the nano-hexapod platform in the direction of the corresponding actuator for all 6 pairs of actuator/sensor (figure [[fig:ine_plant]]).
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i=1:6
+    plot(freqs, abs(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i=1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/ine_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:ine_plant
+#+CAPTION: Transfer function from forces applied in the legs to leg velocity sensor ([[./figs/ine_plant.png][png]], [[./figs/ine_plant.pdf][pdf]])
+[[file:figs/ine_plant.png]]
+
+*** Control Design
+The controller is defined below and the obtained loop gain is shown in figure [[fig:ine_open_loop_gain]].
+
+#+begin_src matlab
+  K_ine = 1e3/(1+s/(2*pi*100));
+#+end_src
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i=1:6
+    plot(freqs, abs(squeeze(freqresp(K_ine*G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i=1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(K_ine*G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/ine_open_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:ine_open_loop_gain
+#+CAPTION: Loop Gain for Inertial Control ([[./figs/ine_open_loop_gain.png][png]], [[./figs/ine_open_loop_gain.pdf][pdf]])
+[[file:figs/ine_open_loop_gain.png]]
+
+*** Diagonal Controller
+We create the diagonal controller and we add a minus sign as we have a positive feedback architecture.
+#+begin_src matlab
+  K_ine = -K_ine*eye(6);
+#+end_src
+
+We save the controller for further analysis.
+#+begin_src matlab
+  save('./active_damping/mat/K_ine.mat', 'K_ine');
+#+end_src
+
+** TODO Identification of the damped plant                         :noexport:
+*** Initialize the Simulation
+Let's initialize the system prior to identification.
+#+begin_src matlab
+  initializeReferences();
+  initializeGround();
+  initializeGranite();
+  initializeTy();
+  initializeRy();
+  initializeRz();
+  initializeMicroHexapod();
+  initializeAxisc();
+  initializeMirror();
+  initializeNanoHexapod('actuator', 'piezo');
+  initializeSample('mass', 50);
+#+end_src
+
+And initialize the controllers.
+#+begin_src matlab
+  K = tf(zeros(6));
+  save('./mat/controllers.mat', 'K', '-append');
+  K_ine = -K_ine*eye(6);
+  save('./mat/controllers.mat', 'K_ine', '-append');
+  K_iff = tf(zeros(6));
+  save('./mat/controllers.mat', 'K_iff', '-append');
+  K_dvf = tf(zeros(6));
+  save('./mat/controllers.mat', 'K_dvf', '-append');
+#+end_src
+
+*** Identification
+We identify the system dynamics now that the Inertial controller is ON.
+#+begin_src matlab
+  G_ine = identifyPlant();
+#+end_src
+
+And we save the damped plant for further analysis.
+#+begin_src matlab
+  save('./active_damping/mat/plants.mat', 'G_ine', '-append');
+#+end_src
+
+*** Sensitivity to disturbances
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  subplot(2, 1, 1);
+  title('$D_g$ to $D$');
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
+  legend('location', 'northeast');
+
+  subplot(2, 1, 2);
+  title('$F_s$ to $D$');
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+  legend('location', 'northeast');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/sensitivity_dist_ine.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:sensitivity_dist_ine
+#+CAPTION: Sensitivity to disturbance once the INE controller is applied to the system ([[./figs/sensitivity_dist_ine.png][png]], [[./figs/sensitivity_dist_ine.pdf][pdf]])
+[[file:figs/sensitivity_dist_ine.png]]
+
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+  legend('location', 'northeast');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/sensitivity_dist_stages_ine.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:sensitivity_dist_stages_ine
+#+CAPTION: Sensitivity to force disturbances in various stages when INE is applied ([[./figs/sensitivity_dist_stages_ine.png][png]], [[./figs/sensitivity_dist_stages_ine.pdf][pdf]])
+[[file:figs/sensitivity_dist_stages_ine.png]]
+
+*** Damped Plant
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 2, 1);
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
+  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
+  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+
+  ax2 = subplot(2, 2, 2);
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
+  plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
+  plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
+
+  ax3 = subplot(2, 2, 3);
+  hold on;
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northwest');
+
+  ax4 = subplot(2, 2, 4);
+  hold on;
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northwest');
+
+  linkaxes([ax1,ax2,ax3,ax4],'x');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/plant_ine_damped.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:plant_ine_damped
+#+CAPTION: Damped Plant after INE is applied ([[./figs/plant_ine_damped.png][png]], [[./figs/plant_ine_damped.pdf][pdf]])
+[[file:figs/plant_ine_damped.png]]
+
+** Tomography Experiment
+*** Initialize the Simulation
+We initialize elements for the tomography experiment.
+#+begin_src matlab
+  prepareTomographyExperiment();
+#+end_src
+
+We set the Inertial controller.
+#+begin_src matlab
+  load('./active_damping/mat/K_ine.mat', 'K_ine');
+  save('./mat/controllers.mat', 'K_ine', '-append');
+#+end_src
+
+*** Simulation
+We change the simulation stop time.
+#+begin_src matlab
+  load('mat/conf_simscape.mat');
+  set_param(conf_simscape, 'StopTime', '3');
+#+end_src
+
+And we simulate the system.
+#+begin_src matlab
+  sim('sim_nass_active_damping');
+#+end_src
+
+Finally, we save the simulation results for further analysis
+#+begin_src matlab
+  En_ine = En;
+  Eg_ine = Eg;
+  save('./active_damping/mat/tomo_exp.mat', 'En_ine', 'Eg_ine', '-append');
+#+end_src
+
+*** Compare with Undamped system
+We load the results of tomography experiments.
+#+begin_src matlab
+  load('./active_damping/mat/tomo_exp.mat', 'En', 'En_ine');
+  t = linspace(0, 3, length(En_ine(:,1)));
+#+end_src
+
+#+begin_src matlab :exports none
+  figure;
+  hold on;
+  plot(t, En(:,1), 'DisplayName', '$\epsilon_{x}$')
+  plot(t, En(:,2), 'DisplayName', '$\epsilon_{y}$')
+  plot(t, En(:,3), 'DisplayName', '$\epsilon_{z}$')
+  set(gca,'ColorOrderIndex',1);
+  plot(t, En_ine(:,1), '--', 'DisplayName', '$\epsilon_{x}$ - Inertial')
+  plot(t, En_ine(:,2), '--', 'DisplayName', '$\epsilon_{y}$ - Inertial')
+  plot(t, En_ine(:,3), '--', 'DisplayName', '$\epsilon_{z}$ - Inertial')
+  hold off;
+  legend();
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/nass_act_damp_ine_sim_tomo_trans.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+NAME: fig:nass_act_damp_ine_sim_tomo_trans
+#+CAPTION: Position Error during tomography experiment - Translations ([[./figs/nass_act_damp_ine_sim_tomo_trans.png][png]], [[./figs/nass_act_damp_ine_sim_tomo_trans.pdf][pdf]])
+[[file:figs/nass_act_damp_ine_sim_tomo_trans.png]]
+
+#+begin_src matlab :exports none
+  figure;
+  hold on;
+  plot(t, En(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
+  plot(t, En(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
+  plot(t, En(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
+  set(gca,'ColorOrderIndex',1);
+  plot(t, En_ine(:,4), '--', 'DisplayName', '$\epsilon_{\theta_x}$ - Inertial')
+  plot(t, En_ine(:,5), '--', 'DisplayName', '$\epsilon_{\theta_y}$ - Inertial')
+  plot(t, En_ine(:,6), '--', 'DisplayName', '$\epsilon_{\theta_z}$ - Inertial')
+  hold off;
+  xlim([0.5,inf]);
+  legend();
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/nass_act_damp_ine_sim_tomo_rot.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+NAME: fig:nass_act_damp_ine_sim_tomo_rot
+#+CAPTION: Position Error during tomography experiment - Rotations ([[./figs/nass_act_damp_ine_sim_tomo_rot.png][png]], [[./figs/nass_act_damp_ine_sim_tomo_rot.pdf][pdf]])
+[[file:figs/nass_act_damp_ine_sim_tomo_rot.png]]
+
+** Conclusion
+#+begin_important
+Inertial Control:
+#+end_important
+
+* Comparison
+<<sec:comparison>>
+** Introduction                                                     :ignore:
+** 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
+  cd('../');
+#+end_src
+
+** Load the plants
+#+begin_src matlab
+  load('./active_damping/mat/plants.mat', 'G', 'G_iff', 'G_ine', 'G_dvf');
+#+end_src
+
+** Sensitivity to Disturbance
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+  title('$D_{g,z}$ to $D_z$');
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_gm(    'Dz', 'Dgz'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
+  plot(freqs, abs(squeeze(freqresp(G_iff.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'INE');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
+  legend('location', 'northeast');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/sensitivity_comp_ground_motion_z.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:sensitivity_comp_ground_motion_z
+#+CAPTION: caption ([[./figs/sensitivity_comp_ground_motion_z.png][png]], [[./figs/sensitivity_comp_ground_motion_z.pdf][pdf]])
+[[file:figs/sensitivity_comp_ground_motion_z.png]]
+
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+  title('$F_{s,z}$ to $D_z$');
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_fs(    'Dz', 'Fsz'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
+  plot(freqs, abs(squeeze(freqresp(G_iff.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'INE');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+  legend('location', 'northeast');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/sensitivity_comp_direct_forces_z.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:sensitivity_comp_direct_forces_z
+#+CAPTION: caption ([[./figs/sensitivity_comp_direct_forces_z.png][png]], [[./figs/sensitivity_comp_direct_forces_z.pdf][pdf]])
+[[file:figs/sensitivity_comp_direct_forces_z.png]]
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+  title('$F_{rz,z}$ to $D_z$');
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_dist(    'Dz', 'Frzz'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
+  plot(freqs, abs(squeeze(freqresp(G_iff.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'INE');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+  legend('location', 'northeast');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/sensitivity_comp_spindle_z.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:sensitivity_comp_spindle_z
+#+CAPTION: caption ([[./figs/sensitivity_comp_spindle_z.png][png]], [[./figs/sensitivity_comp_spindle_z.pdf][pdf]])
+[[file:figs/sensitivity_comp_spindle_z.png]]
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+  title('$F_{ty,z}$ to $D_z$');
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_dist(    'Dz', 'Ftyz'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
+  plot(freqs, abs(squeeze(freqresp(G_iff.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'INE');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+  legend('location', 'northeast');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/sensitivity_comp_ty_z.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:sensitivity_comp_ty_z
+#+CAPTION: caption ([[./figs/sensitivity_comp_ty_z.png][png]], [[./figs/sensitivity_comp_ty_z.pdf][pdf]])
+[[file:figs/sensitivity_comp_ty_z.png]]
+
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+  title('$F_{ty,x}$ to $D_x$');
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_dist(    'Dx', 'Ftyx'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
+  plot(freqs, abs(squeeze(freqresp(G_iff.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'k--', 'DisplayName', 'INE');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+  legend('location', 'northeast');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/sensitivity_comp_ty_x.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:sensitivity_comp_ty_x
+#+CAPTION: caption ([[./figs/sensitivity_comp_ty_x.png][png]], [[./figs/sensitivity_comp_ty_x.pdf][pdf]])
+[[file:figs/sensitivity_comp_ty_x.png]]
+
+** Damped Plant
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  title('$F_{n,z}$ to $D_z$');
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_cart(    'Dz', 'Fnz'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
+  plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'INE');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  legend('location', 'northeast');
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart    ('Dz', 'Fnz'), freqs, 'Hz'))), 'k-');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k:');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k--');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k-.');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/plant_comp_damping_z.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:plant_comp_damping_z
+#+CAPTION: Plant for the $z$ direction for different active damping technique used ([[./figs/plant_comp_damping_z.png][png]], [[./figs/plant_comp_damping_z.pdf][pdf]])
+[[file:figs/plant_comp_damping_z.png]]
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  title('$F_{n,z}$ to $D_z$');
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_cart(    'Dx', 'Fnx'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
+  plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k--', 'DisplayName', 'INE');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  legend('location', 'northeast');
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart    ('Dx', 'Fnx'), freqs, 'Hz'))), 'k-');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k:');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k--');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k-.');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/plant_comp_damping_x.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:plant_comp_damping_x
+#+CAPTION: Plant for the $x$ direction for different active damping technique used ([[./figs/plant_comp_damping_x.png][png]], [[./figs/plant_comp_damping_x.pdf][pdf]])
+[[file:figs/plant_comp_damping_x.png]]
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  title('$F_{n,x}$ to $R_z$');
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_cart(    'Rz', 'Fnx'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
+  plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Rz', 'Fnx'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
+  plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Rz', 'Fnx'), freqs, 'Hz'))), 'k--', 'DisplayName', 'INE');
+  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rz', 'Fnx'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  legend('location', 'northeast');
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart    ('Ry', 'Fnx'), freqs, 'Hz'))), 'k-');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Ry', 'Fnx'), freqs, 'Hz'))), 'k:');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Ry', 'Fnx'), freqs, 'Hz'))), 'k--');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Ry', 'Fnx'), freqs, 'Hz'))), 'k-.');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/plant_comp_damping_coupling.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:plant_comp_damping_coupling
+#+CAPTION: Comparison of one off-diagonal plant for different damping technique applied ([[./figs/plant_comp_damping_coupling.png][png]], [[./figs/plant_comp_damping_coupling.pdf][pdf]])
+[[file:figs/plant_comp_damping_coupling.png]]
+** Tomography Experiment
+
+#+begin_src matlab
+  load('./active_damping/mat/tomo_exp.mat', 'En', 'En_iff', 'En_dvf', 'En_ine');
+  t = linspace(0, 3, length(En(:,1)));
+#+end_src
+
+#+begin_src matlab
+  rms(sqrt(En(:, 1).^2     + En(:, 2).^2     + En(:, 3).^2))
+  rms(sqrt(En_ine(:, 1).^2 + En_ine(:, 2).^2 + En_ine(:, 3).^2))
+  rms(sqrt(En_dvf(:, 1).^2 + En_dvf(:, 2).^2 + En_dvf(:, 3).^2))
+  rms(sqrt(En_iff(:, 1).^2 + En_iff(:, 2).^2 + En_iff(:, 3).^2))
+#+end_src
+
+*** Frequency Domain
+#+begin_src matlab
+  Ts = t(1); % Sample Time for the Data [s]
+
+  n_av = 8;
+  han_win = hanning(ceil(length(En(:, 1))/n_av));
+
+  [pdx, f] = pwelch(Ern(:, 1), han_win, [], [], 1/Ts);
+#+end_src
+
+* Useful Functions
+** prepareTomographyExperiment
+:PROPERTIES:
+:header-args:matlab+: :tangle src/prepareTomographyExperiment.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:prepareTomographyExperiment>>
+
+This Matlab function is accessible [[file:src/prepareTomographyExperiment.m][here]].
+
+*** Function Description
+#+begin_src matlab
+  function [] = prepareTomographyExperiment(args)
+#+end_src
+
+*** Optional Parameters
+#+begin_src matlab
+  arguments
+      args.nass_actuator       char   {mustBeMember(args.nass_actuator,{'piezo', 'lorentz'})} = 'piezo'
+      args.sample_mass   (1,1) double {mustBeNumeric, mustBePositive} = 50
+      args.Ry_period     (1,1) double {mustBeNumeric, mustBePositive} = 1
+  end
+#+end_src
+
+*** Initialize the Simulation
+We initialize all the stages with the default parameters.
+#+begin_src matlab
+  initializeGround();
+  initializeGranite();
+  initializeTy();
+  initializeRy();
+  initializeRz();
+  initializeMicroHexapod();
+  initializeAxisc();
+  initializeMirror();
+#+end_src
+
+The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
+#+begin_src matlab
+  initializeNanoHexapod('actuator', args.nass_actuator);
+  initializeSample('mass', args.sample_mass);
+#+end_src
+
+We set the references to zero.
+#+begin_src matlab
+  initializeReferences('Rz_type', 'rotating', 'Rz_period', args.Ry_period);
+#+end_src
+
+And all the controllers are set to 0.
+#+begin_src matlab
+  K = tf(zeros(6));
+  save('./mat/controllers.mat', 'K', '-append');
+  K_ine = tf(zeros(6));
+  save('./mat/controllers.mat', 'K_ine', '-append');
+  K_iff = tf(zeros(6));
+  save('./mat/controllers.mat', 'K_iff', '-append');
+  K_dvf = tf(zeros(6));
+  save('./mat/controllers.mat', 'K_dvf', '-append');
+#+end_src
+
+* TODO Order                                                        :noexport:
+** Undamped
+*** Identification of the transfer function from disturbance to position error
+#+begin_src matlab
+  %% Options for Linearized
+  options = linearizeOptions;
+  options.SampleTime = 0;
+
+  %% Name of the Simulink File
+  mdl = 'sim_nass_active_damping';
+
+  %% Input/Output definition
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwx'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwy'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1;
+
+  %% Run the linearization
+  G = linearize(mdl, io, options);
+  G.InputName  = {'Dwx', 'Dwy', 'Dwz'};
+  G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
+#+end_src
+
+*** Identification of the plant
+#+begin_src matlab
+  %% Options for Linearized
+  options = linearizeOptions;
+  options.SampleTime = 0;
+
+  %% Name of the Simulink File
+  mdl = 'sim_nass_active_damping';
+
+  %% Input/Output definition
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1;
+
+  %% Run the linearization
+  G = linearize(mdl, io, options);
+  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+  G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
+#+end_src
+
+#+begin_src matlab
+  lzoad('mat/stages.mat', 'nano_hexapod');
+  G_cart = G*inv(nano_hexapod.J');
+  G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
+#+end_src
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G_cart('Edx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$T_{x}$');
+  plot(freqs, abs(squeeze(freqresp(G_cart('Edy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$T_{y}$');
+  plot(freqs, abs(squeeze(freqresp(G_cart('Edz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$T_{z}$');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  legend('location', 'southwest')
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Edx', 'Fnx'), freqs, 'Hz'))));
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Edy', 'Fny'), freqs, 'Hz'))));
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Edz', 'Fnz'), freqs, 'Hz'))));
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$R_{x}$');
+  plot(freqs, abs(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$R_{y}$');
+  plot(freqs, abs(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$R_{z}$');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  legend('location', 'southwest')
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))));
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))));
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))));
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+*** TODO test
+#+begin_src matlab
+  %% Options for Linearized
+  options = linearizeOptions;
+  options.SampleTime = 0;
+
+  %% Name of the Simulink File
+  mdl = 'sim_nass_active_damping';
+
+  %% Input/Output definition
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Micro-Station/Dy'], 1, 'openinput'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1;
+
+  %% Run the linearization
+  G = linearize(mdl, io, options);
+  G.InputName  = {'Dy'};
+  G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
+#+end_src
+
+#+begin_important
+Why is the transfer function from Ty displacement to position error is equal to
+1 at all frequencies?
+Why don't we see any resonance?
+#+end_important
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))), 'DisplayName', '$T_{x}$');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  legend('location', 'southwest')
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))));
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+*** TODO test on hexapod
+#+begin_src matlab
+  %% Options for Linearized
+  options = linearizeOptions;
+  options.SampleTime = 0;
+
+  %% Name of the Simulink File
+  mdl = 'test_nano_hexapod';
+
+  %% Input/Output definition
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/x'],  1, 'openoutput'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/y'],  1, 'openoutput'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/z'],  1, 'openoutput'); io_i = io_i + 1;
+
+  %% Run the linearization
+  G = linearize(mdl, io, options);
+  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+  G.OutputName = {'x', 'y', 'z'};
+#+end_src
+
+#+begin_src matlab
+  %% Options for Linearized
+  options = linearizeOptions;
+  options.SampleTime = 0;
+
+  %% Name of the Simulink File
+  mdl = 'test_nano_hexapod';
+
+  %% Input/Output definition
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Fx'], 1, 'openinput'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/x'], 1, 'openoutput'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;
+
+  %% Run the linearization
+  G = linearize(mdl, io, options);
+  G.InputName  = {'Fx'};
+  G.OutputName = {'x', 'y', 'z'};
+#+end_src
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))), 'DisplayName', '$T_{x}$');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  legend('location', 'southwest')
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))));
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$R_{x}$');
+  plot(freqs, abs(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$R_{y}$');
+  plot(freqs, abs(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$R_{z}$');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  legend('location', 'southwest')
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))));
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))));
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))));
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+*** Sensitivity to disturbances
+The sensitivity to disturbances are shown on figure [[fig:sensitivity_dist_undamped]].
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  subplot(2, 1, 1);
+  title('$D_g$ to $D$');
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
+  legend('location', 'southeast');
+
+  subplot(2, 1, 2);
+  title('$F_s$ to $D$');
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+  legend('location', 'northeast');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/sensitivity_dist_undamped.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:sensitivity_dist_undamped
+#+CAPTION: Undamped sensitivity to disturbances ([[./figs/sensitivity_dist_undamped.png][png]], [[./figs/sensitivity_dist_undamped.pdf][pdf]])
+[[file:figs/sensitivity_dist_undamped.png]]
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
+  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+  legend('location', 'northeast');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/sensitivity_dist_stages.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:sensitivity_dist_stages
+#+CAPTION: Sensitivity to force disturbances in various stages ([[./figs/sensitivity_dist_stages.png][png]], [[./figs/sensitivity_dist_stages.pdf][pdf]])
+[[file:figs/sensitivity_dist_stages.png]]
+*** Undamped Plant
+The "plant" (transfer function from forces applied by the nano-hexapod to the measured displacement of the sample with respect to the granite) bode plot is shown on figure [[fig:sensitivity_dist_undamped]].
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
+  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
+  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$D_x / F_x$');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$D_y / F_y$');
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$D_z / F_z$');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'southwest');
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/plant_undamped.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:plant_undamped
+#+CAPTION: Transfer Function from cartesian forces to displacement for the undamped plant ([[./figs/plant_undamped.png][png]], [[./figs/plant_undamped.pdf][pdf]])
+[[file:figs/plant_undamped.png]]
+
+
+** Direct Velocity Feedback
+*** One degree-of-freedom example
 :PROPERTIES:
 :header-args:matlab+: :tangle no
 :END:
 <<sec:rmc_1dof>>
-*** Equations
+**** Equations
 #+begin_src latex :file rmc_1dof.pdf :post pdf2svg(file=*this*, ext="png") :exports results
   \begin{tikzpicture}
     % Ground
@@ -1071,7 +2519,7 @@ This corresponds to a gain:
   g = \sqrt{km} - c
 \end{equation}
 
-*** Matlab Init                                           :noexport:ignore:
+**** 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
@@ -1080,7 +2528,7 @@ This corresponds to a gain:
   <<matlab-init>>
 #+end_src
 
-*** Matlab Example
+**** Matlab Example
 Let define the system parameters.
 #+begin_src matlab
   m = 50; % [kg]
@@ -1170,336 +2618,14 @@ And the closed loop system is computed below.
 #+NAME: fig:rmc_1dof_sensitivitiy
 #+CAPTION: Sensitivity to disturbance when RMC is applied on the 1dof system ([[./figs/rmc_1dof_sensitivitiy.png][png]], [[./figs/rmc_1dof_sensitivitiy.pdf][pdf]])
 [[file:figs/rmc_1dof_sensitivitiy.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
-  simulinkproject('../');
-#+end_src
-
-#+begin_src matlab
-  open('active_damping/matlab/sim_nano_station_id.slx')
-#+end_src
-
-** Control Design
-Let's load the undamped plant:
-#+begin_src matlab
-  load('./active_damping/mat/plants.mat', 'G');
-#+end_src
-
-Let's look at the transfer function from actuator forces in the nano-hexapod to the measured displacement of the actuator for all 6 pairs of actuator/sensor (figure [[fig:rmc_plant]]).
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  ax1 = subplot(2, 1, 1);
-  hold on;
-  for i=1:6
-    plot(freqs, abs(squeeze(freqresp(G.G_dleg(['Dm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
-  end
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-
-  ax2 = subplot(2, 1, 2);
-  hold on;
-  for i=1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_dleg(['Dm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
-  end
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-
-  linkaxes([ax1,ax2],'x');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/rmc_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:rmc_plant
-#+CAPTION: Transfer function from forces applied in the legs to leg displacement sensor ([[./figs/rmc_plant.png][png]], [[./figs/rmc_plant.pdf][pdf]])
-[[file:figs/rmc_plant.png]]
-
-The Relative Motion Controller is defined below.
-A Low pass Filter is added to make the controller transfer function proper.
-#+begin_src matlab
-  K_rmc = s*50000/(1 + s/2/pi/10000);
-#+end_src
-
-The obtained loop gains are shown in figure [[fig:rmc_open_loop]].
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  ax1 = subplot(2, 1, 1);
-  hold on;
-  for i=1:6
-    plot(freqs, abs(squeeze(freqresp(K_rmc*G.G_dleg(['Dm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
-  end
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-
-  ax2 = subplot(2, 1, 2);
-  hold on;
-  for i=1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(K_rmc*G.G_dleg(['Dm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
-  end
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-
-  linkaxes([ax1,ax2],'x');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/rmc_open_loop.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:rmc_open_loop
-#+CAPTION: Loop Gain for the Integral Force Feedback ([[./figs/rmc_open_loop.png][png]], [[./figs/rmc_open_loop.pdf][pdf]])
-[[file:figs/rmc_open_loop.png]]
-
-** Identification of the damped plant
-Let's initialize the system prior to identification.
-#+begin_src matlab
-  initializeReferences();
-  initializeGround();
-  initializeGranite();
-  initializeTy();
-  initializeRy();
-  initializeRz();
-  initializeMicroHexapod();
-  initializeAxisc();
-  initializeMirror();
-  initializeNanoHexapod('actuator', 'piezo');
-  initializeSample('mass', 50);
-#+end_src
-
-And initialize the controllers.
-#+begin_src matlab
-  K = tf(zeros(6));
-  save('./mat/controllers.mat', 'K', '-append');
-  K_iff = tf(zeros(6));
-  save('./mat/controllers.mat', 'K_iff', '-append');
-  K_rmc = -K_rmc*eye(6);
-  save('./mat/controllers.mat', 'K_rmc', '-append');
-  K_dvf = tf(zeros(6));
-  save('./mat/controllers.mat', 'K_dvf', '-append');
-#+end_src
-
-We identify the system dynamics now that the RMC controller is ON.
-#+begin_src matlab
-  G_rmc = identifyPlant();
-#+end_src
-
-And we save the damped plant for further analysis.
-#+begin_src matlab
-  save('./active_damping/mat/plants.mat', 'G_rmc', '-append');
-#+end_src
-
-** Sensitivity to disturbances
-As shown in figure [[fig:sensitivity_dist_rmc]], RMC control succeed in lowering the sensitivity to disturbances near resonance of the system.
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  subplot(2, 1, 1);
-  title('$D_g$ to $D$');
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
-  set(gca,'ColorOrderIndex',1);
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
-  legend('location', 'southeast');
-
-  subplot(2, 1, 2);
-  title('$F_s$ to $D$');
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
-  set(gca,'ColorOrderIndex',1);
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
-  legend('location', 'northeast');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/sensitivity_dist_rmc.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:sensitivity_dist_rmc
-#+CAPTION: Sensitivity to disturbance once the RMC controller is applied to the system ([[./figs/sensitivity_dist_rmc.png][png]], [[./figs/sensitivity_dist_rmc.pdf][pdf]])
-[[file:figs/sensitivity_dist_rmc.png]]
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
-  set(gca,'ColorOrderIndex',1);
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
-  legend('location', 'northeast');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/sensitivity_dist_stages_rmc.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:sensitivity_dist_stages_rmc
-#+CAPTION: Sensitivity to force disturbances in various stages when RMC is applied ([[./figs/sensitivity_dist_stages_rmc.png][png]], [[./figs/sensitivity_dist_stages_rmc.pdf][pdf]])
-[[file:figs/sensitivity_dist_stages_rmc.png]]
-
-** Damped Plant
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  ax1 = subplot(2, 2, 1);
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
-  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
-  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
-  set(gca,'ColorOrderIndex',1);
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
-
-  ax2 = subplot(2, 2, 2);
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
-  plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
-  plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
-  set(gca,'ColorOrderIndex',1);
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
-
-  ax3 = subplot(2, 2, 3);
-  hold on;
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
-  set(gca,'ColorOrderIndex',1);
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-  legend('location', 'northwest');
-
-  ax4 = subplot(2, 2, 4);
-  hold on;
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
-  set(gca,'ColorOrderIndex',1);
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-  legend('location', 'northwest');
-
-  linkaxes([ax1,ax2,ax3,ax4],'x');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/plant_rmc_damped.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:plant_rmc_damped
-#+CAPTION: Damped Plant after RMC is applied ([[./figs/plant_rmc_damped.png][png]], [[./figs/plant_rmc_damped.pdf][pdf]])
-[[file:figs/plant_rmc_damped.png]]
-
-** Conclusion
-#+begin_important
-Relative Motion Control:
--
-#+end_important
-
-* Direct Velocity Feedback
-:PROPERTIES:
-:header-args:matlab+: :tangle matlab/dvf.m
-:header-args:matlab+: :comments none :mkdirp yes
-:END:
-<<sec:dvf>>
-
-** ZIP file containing the data and matlab files                     :ignore:
-#+begin_src bash :exports none :results none
-  if [ matlab/dvf.m -nt data/dvf.zip ]; then
-    cp matlab/dvf.m dvf.m;
-    zip data/dvf \
-        mat/plant.mat \
-        dvf.m
-    rm dvf.m;
-  fi
-#+end_src
-
-#+begin_note
-  All the files (data and Matlab scripts) are accessible [[file:data/dvf.zip][here]].
-#+end_note
-
-** Introduction                                                     :ignore:
-In the Relative Motion Control (RMC), a feedback is applied between the measured velocity of the platform to the actuator force input.
-
-** One degree-of-freedom example
+** Inertial Control
+*** One degree-of-freedom example
 :PROPERTIES:
 :header-args:matlab+: :tangle no
 :END:
-<<sec:dvf_1dof>>
-*** Equations
-#+begin_src latex :file dvf_1dof.pdf :post pdf2svg(file=*this*, ext="png") :exports results
+<<sec:ine_1dof>>
+**** Equations
+#+begin_src latex :file ine_1dof.pdf :post pdf2svg(file=*this*, ext="png") :exports results
   \begin{tikzpicture}
     % Ground
     \draw (-1, 0) -- (1, 0);
@@ -1525,16 +2651,16 @@ In the Relative Motion Control (RMC), a feedback is applied between the measured
     \draw[actuator={0.4}{0.2}] (0.8, 0) -- (0.8, 1)  coordinate[midway, right=0.1](F);
 
     % Control
-    \node[block={0.8cm}{0.6cm}, right=0.6 of F] (Kdvf) {$K$};
-    \draw[->] (Kdvf.west) -- (F) node[above right]{$F$};
-    \draw[<-] (Kdvf.east) -- ++(0.5, 0) |- (velg.east);
+    \node[block={0.8cm}{0.6cm}, right=0.6 of F] (Kine) {$K$};
+    \draw[->] (Kine.west) -- (F) node[above right]{$F$};
+    \draw[<-] (Kine.east) -- ++(0.5, 0) |- (velg.east);
   \end{tikzpicture}
 #+end_src
 
-#+name: fig:dvf_1dof
+#+name: fig:ine_1dof
 #+caption: Direct Velocity Feedback applied to a 1dof system
 #+RESULTS:
-[[file:figs/dvf_1dof.png]]
+[[file:figs/ine_1dof.png]]
 
 The dynamic of the system is:
 \begin{equation}
@@ -1544,7 +2670,7 @@ In terms of the stage deformation $d = x - w$:
 \begin{equation}
   (ms^2 + cs + k) d = -ms^2 w + F_d + F
 \end{equation}
-The direct velocity feedback law shown in figure [[fig:dvf_1dof]] is:
+The direct velocity feedback law shown in figure [[fig:ine_1dof]] is:
 \begin{equation}
   K = -g
 \end{equation}
@@ -1568,7 +2694,7 @@ This corresponds to a gain:
   g = \sqrt{km} - c
 \end{equation}
 
-*** Matlab Init                                           :noexport:ignore:
+**** 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
@@ -1577,7 +2703,7 @@ This corresponds to a gain:
   <<matlab-init>>
 #+end_src
 
-*** Matlab Example
+**** Matlab Example
 Let define the system parameters.
 #+begin_src matlab
   m = 50; % [kg]
@@ -1620,19 +2746,19 @@ Finally, the system is described by =sys= as defined below.
   sys = series(sys, G_xdot, [1 2 3], [1 2 3]);
 #+end_src
 
-The controller $K_\text{DVF}$ is:
+The controller $K_\text{INE}$ is:
 #+begin_src matlab
-  Kdvf = tf(-(sqrt(k*m)-c));
-  Kdvf.InputName = {'xdot'};
-  Kdvf.OutputName = {'F'};
+  Kine = tf(-(sqrt(k*m)-c));
+  Kine.InputName = {'xdot'};
+  Kine.OutputName = {'F'};
 #+end_src
 
 And the closed loop system is computed below.
 #+begin_src matlab
-  sys_dvf = feedback(sys, Kdvf, 'name', +1);
+  sys_ine = feedback(sys, Kine, 'name', +1);
 #+end_src
 
-The obtained sensitivity to disturbances is shown in figure [[fig:dvf_1dof_sensitivitiy]].
+The obtained sensitivity to disturbances is shown in figure [[fig:ine_1dof_sensitivitiy]].
 #+begin_src matlab :exports none
   freqs = logspace(-1, 3, 1000);
 
@@ -1642,7 +2768,7 @@ The obtained sensitivity to disturbances is shown in figure [[fig:dvf_1dof_sensi
   title('Fd to d')
   hold on;
   plot(freqs, abs(squeeze(freqresp(sys('d', 'Fd'), freqs, 'Hz'))));
-  plot(freqs, abs(squeeze(freqresp(sys_dvf('d', 'Fd'), freqs, 'Hz'))));
+  plot(freqs, abs(squeeze(freqresp(sys_ine('d', 'Fd'), freqs, 'Hz'))));
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
   xlim([freqs(1), freqs(end)]);
@@ -1651,7 +2777,7 @@ The obtained sensitivity to disturbances is shown in figure [[fig:dvf_1dof_sensi
   title('Fd to x')
   hold on;
   plot(freqs, abs(squeeze(freqresp(sys('xdot', 'Fd')/s, freqs, 'Hz'))));
-  plot(freqs, abs(squeeze(freqresp(sys_dvf('xdot', 'Fd')/s, freqs, 'Hz'))));
+  plot(freqs, abs(squeeze(freqresp(sys_ine('xdot', 'Fd')/s, freqs, 'Hz'))));
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
   xlim([freqs(1), freqs(end)]);
@@ -1660,7 +2786,7 @@ The obtained sensitivity to disturbances is shown in figure [[fig:dvf_1dof_sensi
   title('w to d')
   hold on;
   plot(freqs, abs(squeeze(freqresp(sys('d', 'wddot')*s^2, freqs, 'Hz'))));
-  plot(freqs, abs(squeeze(freqresp(sys_dvf('d', 'wddot')*s^2, freqs, 'Hz'))));
+  plot(freqs, abs(squeeze(freqresp(sys_ine('d', 'wddot')*s^2, freqs, 'Hz'))));
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
   xlim([freqs(1), freqs(end)]);
@@ -1669,576 +2795,17 @@ The obtained sensitivity to disturbances is shown in figure [[fig:dvf_1dof_sensi
   title('w to x')
   hold on;
   plot(freqs, abs(squeeze(freqresp(1+sys('d', 'wddot')*s^2, freqs, 'Hz'))));
-  plot(freqs, abs(squeeze(freqresp(1+sys_dvf('d', 'wddot')*s^2, freqs, 'Hz'))));
+  plot(freqs, abs(squeeze(freqresp(1+sys_ine('d', 'wddot')*s^2, freqs, 'Hz'))));
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
   xlim([freqs(1), freqs(end)]);
 #+end_src
 
 #+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/dvf_1dof_sensitivitiy.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+#+begin_src matlab :var filepath="figs/ine_1dof_sensitivitiy.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
   <<plt-matlab>>
 #+end_src
 
-#+NAME: fig:dvf_1dof_sensitivitiy
-#+CAPTION: Sensitivity to disturbance when DVF is applied on the 1dof system ([[./figs/dvf_1dof_sensitivitiy.png][png]], [[./figs/dvf_1dof_sensitivitiy.pdf][pdf]])
-[[file:figs/dvf_1dof_sensitivitiy.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
-  simulinkproject('../');
-#+end_src
-
-#+begin_src matlab
-  open('active_damping/matlab/sim_nano_station_id.slx')
-#+end_src
-
-** Control Design
-Let's load the undamped plant:
-#+begin_src matlab
-  load('./active_damping/mat/plants.mat', 'G');
-#+end_src
-
-Let's look at the transfer function from actuator forces in the nano-hexapod to the measured velocity of the nano-hexapod platform in the direction of the corresponding actuator for all 6 pairs of actuator/sensor (figure [[fig:dvf_plant]]).
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  ax1 = subplot(2, 1, 1);
-  hold on;
-  for i=1:6
-    plot(freqs, abs(squeeze(freqresp(G.G_geoph(['Vm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
-  end
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-
-  ax2 = subplot(2, 1, 2);
-  hold on;
-  for i=1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_geoph(['Vm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
-  end
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-
-  linkaxes([ax1,ax2],'x');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/dvf_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:dvf_plant
-#+CAPTION: Transfer function from forces applied in the legs to leg velocity sensor ([[./figs/dvf_plant.png][png]], [[./figs/dvf_plant.pdf][pdf]])
-[[file:figs/dvf_plant.png]]
-
-The controller is defined below and the obtained loop gain is shown in figure [[fig:dvf_open_loop_gain]].
-
-#+begin_src matlab
-  K_dvf = tf(3e4);
-#+end_src
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  ax1 = subplot(2, 1, 1);
-  hold on;
-  for i=1:6
-    plot(freqs, abs(squeeze(freqresp(K_dvf*G.G_geoph(['Vm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
-  end
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-
-  ax2 = subplot(2, 1, 2);
-  hold on;
-  for i=1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(K_dvf*G.G_geoph(['Vm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
-  end
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-
-  linkaxes([ax1,ax2],'x');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/dvf_open_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:dvf_open_loop_gain
-#+CAPTION: Loop Gain for DVF ([[./figs/dvf_open_loop_gain.png][png]], [[./figs/dvf_open_loop_gain.pdf][pdf]])
-[[file:figs/dvf_open_loop_gain.png]]
-
-** Identification of the damped plant
-Let's initialize the system prior to identification.
-#+begin_src matlab
-  initializeReferences();
-  initializeGround();
-  initializeGranite();
-  initializeTy();
-  initializeRy();
-  initializeRz();
-  initializeMicroHexapod();
-  initializeAxisc();
-  initializeMirror();
-  initializeNanoHexapod('actuator', 'piezo');
-  initializeSample('mass', 50);
-#+end_src
-
-And initialize the controllers.
-#+begin_src matlab
-  K = tf(zeros(6));
-  save('./mat/controllers.mat', 'K', '-append');
-  K_iff = tf(zeros(6));
-  save('./mat/controllers.mat', 'K_iff', '-append');
-  K_rmc = tf(zeros(6));
-  save('./mat/controllers.mat', 'K_rmc', '-append');
-  K_dvf = -K_dvf*eye(6);
-  save('./mat/controllers.mat', 'K_dvf', '-append');
-#+end_src
-
-We identify the system dynamics now that the RMC controller is ON.
-#+begin_src matlab
-  G_dvf = identifyPlant();
-#+end_src
-
-And we save the damped plant for further analysis.
-#+begin_src matlab
-  save('./active_damping/mat/plants.mat', 'G_dvf', '-append');
-#+end_src
-
-** Sensitivity to disturbances
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  subplot(2, 1, 1);
-  title('$D_g$ to $D$');
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
-  set(gca,'ColorOrderIndex',1);
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
-  legend('location', 'northeast');
-
-  subplot(2, 1, 2);
-  title('$F_s$ to $D$');
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
-  set(gca,'ColorOrderIndex',1);
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
-  legend('location', 'northeast');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/sensitivity_dist_dvf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:sensitivity_dist_dvf
-#+CAPTION: Sensitivity to disturbance once the DVF controller is applied to the system ([[./figs/sensitivity_dist_dvf.png][png]], [[./figs/sensitivity_dist_dvf.pdf][pdf]])
-[[file:figs/sensitivity_dist_dvf.png]]
-
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
-  plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
-  set(gca,'ColorOrderIndex',1);
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
-  legend('location', 'northeast');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/sensitivity_dist_stages_dvf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:sensitivity_dist_stages_dvf
-#+CAPTION: Sensitivity to force disturbances in various stages when DVF is applied ([[./figs/sensitivity_dist_stages_dvf.png][png]], [[./figs/sensitivity_dist_stages_dvf.pdf][pdf]])
-[[file:figs/sensitivity_dist_stages_dvf.png]]
-
-** Damped Plant
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  ax1 = subplot(2, 2, 1);
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
-  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
-  plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
-  set(gca,'ColorOrderIndex',1);
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
-
-  ax2 = subplot(2, 2, 2);
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
-  plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
-  plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
-  set(gca,'ColorOrderIndex',1);
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
-
-  ax3 = subplot(2, 2, 3);
-  hold on;
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
-  set(gca,'ColorOrderIndex',1);
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-  legend('location', 'northwest');
-
-  ax4 = subplot(2, 2, 4);
-  hold on;
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
-  set(gca,'ColorOrderIndex',1);
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-  legend('location', 'northwest');
-
-  linkaxes([ax1,ax2,ax3,ax4],'x');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/plant_dvf_damped.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:plant_dvf_damped
-#+CAPTION: Damped Plant after DVF is applied ([[./figs/plant_dvf_damped.png][png]], [[./figs/plant_dvf_damped.pdf][pdf]])
-[[file:figs/plant_dvf_damped.png]]
-
-** Conclusion
-#+begin_important
-Direct Velocity Feedback:
-#+end_important
-
-* Comparison
-<<sec:comparison>>
-** Introduction                                                     :ignore:
-** 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
-  cd('../');
-#+end_src
-
-** Load the plants
-#+begin_src matlab
-  load('./active_damping/mat/plants.mat', 'G', 'G_iff', 'G_rmc', 'G_dvf');
-#+end_src
-
-** Sensitivity to Disturbance
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-  title('$D_{g,z}$ to $D_z$');
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_gm(    'Dz', 'Dgz'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
-  plot(freqs, abs(squeeze(freqresp(G_iff.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'RMC');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
-  legend('location', 'northeast');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/sensitivity_comp_ground_motion_z.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:sensitivity_comp_ground_motion_z
-#+CAPTION: caption ([[./figs/sensitivity_comp_ground_motion_z.png][png]], [[./figs/sensitivity_comp_ground_motion_z.pdf][pdf]])
-[[file:figs/sensitivity_comp_ground_motion_z.png]]
-
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-  title('$F_{s,z}$ to $D_z$');
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_fs(    'Dz', 'Fsz'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
-  plot(freqs, abs(squeeze(freqresp(G_iff.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'RMC');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
-  legend('location', 'northeast');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/sensitivity_comp_direct_forces_z.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:sensitivity_comp_direct_forces_z
-#+CAPTION: caption ([[./figs/sensitivity_comp_direct_forces_z.png][png]], [[./figs/sensitivity_comp_direct_forces_z.pdf][pdf]])
-[[file:figs/sensitivity_comp_direct_forces_z.png]]
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-  title('$F_{rz,z}$ to $D_z$');
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_dist(    'Dz', 'Frzz'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
-  plot(freqs, abs(squeeze(freqresp(G_iff.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'RMC');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
-  legend('location', 'northeast');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/sensitivity_comp_spindle_z.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:sensitivity_comp_spindle_z
-#+CAPTION: caption ([[./figs/sensitivity_comp_spindle_z.png][png]], [[./figs/sensitivity_comp_spindle_z.pdf][pdf]])
-[[file:figs/sensitivity_comp_spindle_z.png]]
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-  title('$F_{ty,z}$ to $D_z$');
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_dist(    'Dz', 'Ftyz'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
-  plot(freqs, abs(squeeze(freqresp(G_iff.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'RMC');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
-  legend('location', 'northeast');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/sensitivity_comp_ty_z.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:sensitivity_comp_ty_z
-#+CAPTION: caption ([[./figs/sensitivity_comp_ty_z.png][png]], [[./figs/sensitivity_comp_ty_z.pdf][pdf]])
-[[file:figs/sensitivity_comp_ty_z.png]]
-
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-  title('$F_{ty,x}$ to $D_x$');
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_dist(    'Dx', 'Ftyx'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
-  plot(freqs, abs(squeeze(freqresp(G_iff.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'k--', 'DisplayName', 'RMC');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
-  legend('location', 'northeast');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/sensitivity_comp_ty_x.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:sensitivity_comp_ty_x
-#+CAPTION: caption ([[./figs/sensitivity_comp_ty_x.png][png]], [[./figs/sensitivity_comp_ty_x.pdf][pdf]])
-[[file:figs/sensitivity_comp_ty_x.png]]
-
-** Damped Plant
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  title('$F_{n,z}$ to $D_z$');
-  ax1 = subplot(2, 1, 1);
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_cart(    'Dz', 'Fnz'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
-  plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'RMC');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-  legend('location', 'northeast');
-
-  ax2 = subplot(2, 1, 2);
-  hold on;
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart    ('Dz', 'Fnz'), freqs, 'Hz'))), 'k-');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k:');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k--');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k-.');
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-
-  linkaxes([ax1,ax2],'x');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/plant_comp_damping_z.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:plant_comp_damping_z
-#+CAPTION: Plant for the $z$ direction for different active damping technique used ([[./figs/plant_comp_damping_z.png][png]], [[./figs/plant_comp_damping_z.pdf][pdf]])
-[[file:figs/plant_comp_damping_z.png]]
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  title('$F_{n,z}$ to $D_z$');
-  ax1 = subplot(2, 1, 1);
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_cart(    'Dx', 'Fnx'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
-  plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k--', 'DisplayName', 'RMC');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-  legend('location', 'northeast');
-
-  ax2 = subplot(2, 1, 2);
-  hold on;
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart    ('Dx', 'Fnx'), freqs, 'Hz'))), 'k-');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k:');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k--');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k-.');
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-
-  linkaxes([ax1,ax2],'x');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/plant_comp_damping_x.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:plant_comp_damping_x
-#+CAPTION: Plant for the $x$ direction for different active damping technique used ([[./figs/plant_comp_damping_x.png][png]], [[./figs/plant_comp_damping_x.pdf][pdf]])
-[[file:figs/plant_comp_damping_x.png]]
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  title('$F_{n,x}$ to $R_z$');
-  ax1 = subplot(2, 1, 1);
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(G.G_cart(    'Rz', 'Fnx'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
-  plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Rz', 'Fnx'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
-  plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Rz', 'Fnx'), freqs, 'Hz'))), 'k--', 'DisplayName', 'RMC');
-  plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rz', 'Fnx'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-  legend('location', 'northeast');
-
-  ax2 = subplot(2, 1, 2);
-  hold on;
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart    ('Ry', 'Fnx'), freqs, 'Hz'))), 'k-');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Ry', 'Fnx'), freqs, 'Hz'))), 'k:');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Ry', 'Fnx'), freqs, 'Hz'))), 'k--');
-  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Ry', 'Fnx'), freqs, 'Hz'))), 'k-.');
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-
-  linkaxes([ax1,ax2],'x');
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/plant_comp_damping_coupling.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:plant_comp_damping_coupling
-#+CAPTION: Comparison of one off-diagonal plant for different damping technique applied ([[./figs/plant_comp_damping_coupling.png][png]], [[./figs/plant_comp_damping_coupling.pdf][pdf]])
-[[file:figs/plant_comp_damping_coupling.png]]
+#+NAME: fig:ine_1dof_sensitivitiy
+#+CAPTION: Sensitivity to disturbance when INE is applied on the 1dof system ([[./figs/ine_1dof_sensitivitiy.png][png]], [[./figs/ine_1dof_sensitivitiy.pdf][pdf]])
+[[file:figs/ine_1dof_sensitivitiy.png]]
diff --git a/active_damping/matlab/dvf.m b/active_damping/matlab/dvf.m
new file mode 100644
index 0000000..16df905
--- /dev/null
+++ b/active_damping/matlab/dvf.m
@@ -0,0 +1,280 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+open('active_damping/matlab/sim_nass_active_damping.slx')
+
+load('./active_damping/mat/plants.mat', 'G_dvf');
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 1, 1);
+hold on;
+for i=1:6
+  plot(freqs, abs(squeeze(freqresp(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+ax2 = subplot(2, 1, 2);
+hold on;
+for i=1:6
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2],'x');
+
+K_dvf = s*20000/(1 + s/2/pi/10000);
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 1, 1);
+hold on;
+for i=1:6
+  plot(freqs, abs(squeeze(freqresp(K_dvf*G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+ax2 = subplot(2, 1, 2);
+hold on;
+for i=1:6
+  plot(freqs, 180/pi*angle(squeeze(freqresp(K_dvf*G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2],'x');
+
+K_dvf = -K_dvf*eye(6);
+
+save('./active_damping/mat/K_dvf.mat', 'K_dvf');
+
+initializeReferences();
+initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+initializeAxisc();
+initializeMirror();
+initializeNanoHexapod('actuator', 'piezo');
+initializeSample('mass', 50);
+
+K = tf(zeros(6));
+save('./mat/controllers.mat', 'K', '-append');
+K_iff = tf(zeros(6));
+save('./mat/controllers.mat', 'K_iff', '-append');
+K_dvf = K_dvf;
+save('./mat/controllers.mat', 'K_dvf', '-append');
+K_dvf = tf(zeros(6));
+save('./mat/controllers.mat', 'K_dvf', '-append');
+
+%% Options for Linearized
+options = linearizeOptions;
+options.SampleTime = 0;
+
+%% Name of the Simulink File
+mdl = 'sim_nass_active_damping';
+
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
+
+%% Run the linearization
+G = linearize(mdl, io, options);
+G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
+                'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
+
+save('./active_damping/mat/plants.mat', 'G_dvf', '-append');
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+subplot(2, 1, 1);
+title('$D_g$ to $D$');
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
+legend('location', 'southeast');
+
+subplot(2, 1, 2);
+title('$F_s$ to $D$');
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 2, 1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+
+ax2 = subplot(2, 2, 2);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
+
+ax3 = subplot(2, 2, 3);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+legend('location', 'northwest');
+
+ax4 = subplot(2, 2, 4);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+legend('location', 'northwest');
+
+linkaxes([ax1,ax2,ax3,ax4],'x');
+
+initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+initializeAxisc();
+initializeMirror();
+
+initializeNanoHexapod('actuator', 'piezo');
+initializeSample('mass', 50);
+
+initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
+
+initDisturbances();
+
+K = tf(zeros(6));
+save('./mat/controllers.mat', 'K', '-append');
+K_ine = tf(zeros(6));
+save('./mat/controllers.mat', 'K_ine', '-append');
+K_iff = tf(zeros(6));
+save('./mat/controllers.mat', 'K_iff', '-append');
+K_dvf = K_dvf;
+save('./mat/controllers.mat', 'K_dvf', '-append');
+
+load('mat/conf_simscape.mat');
+set_param(conf_simscape, 'StopTime', '3');
+
+sim('sim_nass_active_damping');
+
+t_dvf = t;
+Ern_dvf = Ern;
+save('./active_damping/mat/tomo_exp.mat', 'Ern_dvf', 't_dvf', '-append');
+
+load('./active_damping/mat/tomo_exp.mat', 'Ern', 'Ern_dvf', 't', 't_dvf');
+
+figure;
+hold on;
+plot(t, Ern(:,1), 'DisplayName', '$\epsilon_{x}$')
+plot(t, Ern(:,2), 'DisplayName', '$\epsilon_{y}$')
+plot(t, Ern(:,3), 'DisplayName', '$\epsilon_{z}$')
+set(gca,'ColorOrderIndex',1);
+plot(t_dvf, Ern_dvf(:,1), '--', 'DisplayName', '$\epsilon_{x}$ - DVF')
+plot(t_dvf, Ern_dvf(:,2), '--', 'DisplayName', '$\epsilon_{y}$ - DVF')
+plot(t_dvf, Ern_dvf(:,3), '--', 'DisplayName', '$\epsilon_{z}$ - DVF')
+hold off;
+xlim([1,inf]);
+legend();
+
+figure;
+hold on;
+plot(t, Ern(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
+plot(t, Ern(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
+plot(t, Ern(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
+set(gca,'ColorOrderIndex',1);
+plot(t_dvf, Ern_dvf(:,4), '--', 'DisplayName', '$\epsilon_{\theta_x}$ - DVF')
+plot(t_dvf, Ern_dvf(:,5), '--', 'DisplayName', '$\epsilon_{\theta_y}$ - DVF')
+plot(t_dvf, Ern_dvf(:,6), '--', 'DisplayName', '$\epsilon_{\theta_z}$ - DVF')
+hold off;
+xlim([1,inf]);
+legend();
diff --git a/active_damping/matlab/iff.m b/active_damping/matlab/iff.m
new file mode 100644
index 0000000..c238bd3
--- /dev/null
+++ b/active_damping/matlab/iff.m
@@ -0,0 +1,300 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+open('active_damping/matlab/sim_nass_active_damping.slx')
+
+load('./active_damping/mat/undamped_plants.mat', 'G_iff');
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 1, 1);
+hold on;
+for i=1:6
+  plot(freqs, abs(squeeze(freqresp(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
+
+ax2 = subplot(2, 1, 2);
+hold on;
+for i=1:6
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2],'x');
+
+K_iff = 1000/s;
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 1, 1);
+hold on;
+for i=1:6
+  plot(freqs, abs(squeeze(freqresp(K_iff*G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
+
+ax2 = subplot(2, 1, 2);
+hold on;
+for i=1:6
+  plot(freqs, 180/pi*angle(squeeze(freqresp(K_iff*G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2],'x');
+
+K_iff = -K_iff*eye(6);
+
+save('./active_damping/mat/K_iff.mat', 'K_iff');
+
+initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+initializeAxisc();
+initializeMirror();
+
+initializeNanoHexapod('actuator', 'piezo');
+initializeSample('mass', 50);
+
+initializeReferences();
+
+K = tf(zeros(6));
+save('./mat/controllers.mat', 'K', '-append');
+K_ine = tf(zeros(6));
+save('./mat/controllers.mat', 'K_ine', '-append');
+K_iff = K_iff;
+save('./mat/controllers.mat', 'K_iff', '-append');
+K_dvf = tf(zeros(6));
+save('./mat/controllers.mat', 'K_dvf', '-append');
+
+%% Options for Linearized
+options = linearizeOptions;
+options.SampleTime = 0;
+
+%% Name of the Simulink File
+mdl = 'sim_nass_active_damping';
+
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/Fnl'],           1, 'openinput');              io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
+
+%% Run the linearization
+G = linearize(mdl, io, options);
+G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
+                'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
+
+G_iff = minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
+% G_rmc = minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
+
+save('./active_damping/mat/plants.mat', 'G_iff', '-append');
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+subplot(2, 1, 1);
+title('$D_g$ to $D$');
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_iff.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_iff.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_iff.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+subplot(2, 1, 2);
+title('$F_s$ to $D$');
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_iff.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_iff.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_iff.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(minreal(prescale(G_iff.G_dist('Dz', 'Frzz'), {2*pi, 2*pi*1e3})), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(minreal(G_iff.G_dist('Dz', 'Ftyz')), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(minreal(G_iff.G_dist('Dx', 'Ftyx')), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 2, 1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+
+ax2 = subplot(2, 2, 2);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
+
+ax3 = subplot(2, 2, 3);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+legend('location', 'northwest');
+
+ax4 = subplot(2, 2, 4);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+legend('location', 'northwest');
+
+linkaxes([ax1,ax2,ax3,ax4],'x');
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+for ix = 1:6
+  for iy = 1:6
+    subplot(6, 6, (ix-1)*6 + iy);
+    hold on;
+    plot(freqs, abs(squeeze(freqresp(G.G_cart(ix, iy), freqs, 'Hz'))), 'k-');
+    plot(freqs, abs(squeeze(freqresp(G_iff.G_cart(ix, iy), freqs, 'Hz'))), 'k--');
+    set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+    ylim([1e-12, 1e-5]);
+  end
+end
+
+initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+initializeAxisc();
+initializeMirror();
+
+initializeNanoHexapod('actuator', 'piezo');
+initializeSample('mass', 50);
+
+initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
+
+initDisturbances();
+
+K = tf(zeros(6));
+save('./mat/controllers.mat', 'K', '-append');
+K_ine = tf(zeros(6));
+save('./mat/controllers.mat', 'K_ine', '-append');
+K_iff = K_iff;
+save('./mat/controllers.mat', 'K_iff', '-append');
+K_dvf = tf(zeros(6));
+save('./mat/controllers.mat', 'K_dvf', '-append');
+
+load('mat/conf_simscape.mat');
+set_param(conf_simscape, 'StopTime', '3');
+
+sim('sim_nass_active_damping');
+
+t_iff = t;
+Ern_iff = Ern;
+save('./active_damping/mat/tomo_exp.mat', 'Ern_iff', 't_iff', '-append');
+
+load('./active_damping/mat/tomo_exp.mat', 'Ern', 'Ern_iff', 't', 't_iff');
+
+figure;
+hold on;
+plot(t, Ern(:,1), 'DisplayName', '$\epsilon_{x}$')
+plot(t, Ern(:,2), 'DisplayName', '$\epsilon_{y}$')
+plot(t, Ern(:,3), 'DisplayName', '$\epsilon_{z}$')
+set(gca,'ColorOrderIndex',1);
+plot(t_iff, Ern_iff(:,1), '--', 'DisplayName', '$\epsilon_{x}$ - IFF')
+plot(t_iff, Ern_iff(:,2), '--', 'DisplayName', '$\epsilon_{y}$ - IFF')
+plot(t_iff, Ern_iff(:,3), '--', 'DisplayName', '$\epsilon_{z}$ - IFF')
+hold off;
+xlim([1,inf]);
+legend();
+
+figure;
+hold on;
+plot(t, Ern(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
+plot(t, Ern(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
+plot(t, Ern(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
+set(gca,'ColorOrderIndex',1);
+plot(t_iff, Ern_iff(:,4), '--', 'DisplayName', '$\epsilon_{\theta_x}$ - IFF')
+plot(t_iff, Ern_iff(:,5), '--', 'DisplayName', '$\epsilon_{\theta_y}$ - IFF')
+plot(t_iff, Ern_iff(:,6), '--', 'DisplayName', '$\epsilon_{\theta_z}$ - IFF')
+hold off;
+xlim([1,inf]);
+legend();
diff --git a/active_damping/matlab/ine.m b/active_damping/matlab/ine.m
new file mode 100644
index 0000000..4c6e624
--- /dev/null
+++ b/active_damping/matlab/ine.m
@@ -0,0 +1,263 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+open('active_damping/matlab/sim_nass_active_damping.slx')
+
+load('./active_damping/mat/undamped_plants.mat', 'G_ine');
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 1, 1);
+hold on;
+for i=1:6
+  plot(freqs, abs(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+ax2 = subplot(2, 1, 2);
+hold on;
+for i=1:6
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2],'x');
+
+K_ine = 1e3/(1+s/(2*pi*100));
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 1, 1);
+hold on;
+for i=1:6
+  plot(freqs, abs(squeeze(freqresp(K_ine*G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+ax2 = subplot(2, 1, 2);
+hold on;
+for i=1:6
+  plot(freqs, 180/pi*angle(squeeze(freqresp(K_ine*G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2],'x');
+
+K_ine = -K_ine*eye(6);
+
+save('./active_damping/mat/K_ine.mat', 'K_ine');
+
+initializeReferences();
+initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+initializeAxisc();
+initializeMirror();
+initializeNanoHexapod('actuator', 'piezo');
+initializeSample('mass', 50);
+
+K = tf(zeros(6));
+save('./mat/controllers.mat', 'K', '-append');
+K_iff = tf(zeros(6));
+save('./mat/controllers.mat', 'K_iff', '-append');
+K_ine = tf(zeros(6));
+save('./mat/controllers.mat', 'K_ine', '-append');
+K_ine = -K_ine*eye(6);
+save('./mat/controllers.mat', 'K_ine', '-append');
+
+G_ine = identifyPlant();
+
+save('./active_damping/mat/plants.mat', 'G_ine', '-append');
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+subplot(2, 1, 1);
+title('$D_g$ to $D$');
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_ine.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_ine.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_ine.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+subplot(2, 1, 2);
+title('$F_s$ to $D$');
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_ine.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_ine.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_ine.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_ine.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_ine.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_ine.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 2, 1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+
+ax2 = subplot(2, 2, 2);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
+
+ax3 = subplot(2, 2, 3);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+legend('location', 'northwest');
+
+ax4 = subplot(2, 2, 4);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+legend('location', 'northwest');
+
+linkaxes([ax1,ax2,ax3,ax4],'x');
+
+initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+initializeAxisc();
+initializeMirror();
+
+initializeNanoHexapod('actuator', 'piezo');
+initializeSample('mass', 50);
+
+initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
+
+initDisturbances();
+
+K = tf(zeros(6));
+save('./mat/controllers.mat', 'K', '-append');
+K_ine = K_ine;
+save('./mat/controllers.mat', 'K_ine', '-append');
+K_iff = tf(zeros(6));
+save('./mat/controllers.mat', 'K_iff', '-append');
+K_dvf = tf(zeros(6));
+save('./mat/controllers.mat', 'K_dvf', '-append');
+
+load('mat/conf_simscape.mat');
+set_param(conf_simscape, 'StopTime', '3');
+
+sim('sim_nass_active_damping');
+
+t_ine = t;
+Ern_ine = Ern;
+save('./active_damping/mat/tomo_exp.mat', 'Ern_ine', 't_ine', '-append');
+
+load('./active_damping/mat/tomo_exp.mat', 'Ern', 'Ern_ine', 't', 't_ine');
+
+figure;
+hold on;
+plot(t, Ern(:,1), 'DisplayName', '$\epsilon_{x}$')
+plot(t, Ern(:,2), 'DisplayName', '$\epsilon_{y}$')
+plot(t, Ern(:,3), 'DisplayName', '$\epsilon_{z}$')
+set(gca,'ColorOrderIndex',1);
+plot(t_ine, Ern_ine(:,1), '--', 'DisplayName', '$\epsilon_{x}$ - Inertial')
+plot(t_ine, Ern_ine(:,2), '--', 'DisplayName', '$\epsilon_{y}$ - Inertial')
+plot(t_ine, Ern_ine(:,3), '--', 'DisplayName', '$\epsilon_{z}$ - Inertial')
+hold off;
+xlim([1,inf]);
+legend();
+
+figure;
+hold on;
+plot(t, Ern(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
+plot(t, Ern(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
+plot(t, Ern(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
+set(gca,'ColorOrderIndex',1);
+plot(t_ine, Ern_ine(:,4), '--', 'DisplayName', '$\epsilon_{\theta_x}$ - Inertial')
+plot(t_ine, Ern_ine(:,5), '--', 'DisplayName', '$\epsilon_{\theta_y}$ - Inertial')
+plot(t_ine, Ern_ine(:,6), '--', 'DisplayName', '$\epsilon_{\theta_z}$ - Inertial')
+hold off;
+xlim([1,inf]);
+legend();
diff --git a/active_damping/matlab/sim_nass_active_damping.slx b/active_damping/matlab/sim_nass_active_damping.slx
index ef9bd96..f2eaf42 100644
Binary files a/active_damping/matlab/sim_nass_active_damping.slx and b/active_damping/matlab/sim_nass_active_damping.slx differ
diff --git a/active_damping/matlab/undamped_system.m b/active_damping/matlab/undamped_system.m
new file mode 100644
index 0000000..e49889d
--- /dev/null
+++ b/active_damping/matlab/undamped_system.m
@@ -0,0 +1,166 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+open('active_damping/matlab/sim_nass_active_damping.slx')
+
+initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+initializeAxisc();
+initializeMirror();
+
+initializeNanoHexapod('actuator', 'piezo');
+initializeSample('mass', 50);
+
+initializeReferences();
+
+K = tf(zeros(6));
+save('./mat/controllers.mat', 'K', '-append');
+K_ine = tf(zeros(6));
+save('./mat/controllers.mat', 'K_ine', '-append');
+K_iff = tf(zeros(6));
+save('./mat/controllers.mat', 'K_iff', '-append');
+K_dvf = tf(zeros(6));
+save('./mat/controllers.mat', 'K_dvf', '-append');
+
+%% Options for Linearized
+options = linearizeOptions;
+options.SampleTime = 0;
+
+%% Name of the Simulink File
+mdl = 'sim_nass_active_damping';
+
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/Fnl'],           1, 'openinput');              io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vlm');  io_i = io_i + 1;
+
+%% Run the linearization
+G = linearize(mdl, io, options);
+G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
+                'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
+                'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'};
+
+G_iff = minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
+G_dvf = minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
+G_ine = minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
+
+save('./active_damping/mat/undamped_plants.mat', 'G_iff', 'G_dvf', 'G_ine');
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 1, 1);
+hold on;
+for i = 1:6
+  plot(freqs, abs(squeeze(freqresp(G(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+ax2 = subplot(2, 1, 2);
+hold on;
+for i = 1:6
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2],'x');
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 1, 1);
+hold on;
+for i = 1:6
+  plot(freqs, abs(squeeze(freqresp(G(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+ax2 = subplot(2, 1, 2);
+hold on;
+for i = 1:6
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2],'x');
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 1, 1);
+hold on;
+for i = 1:6
+  plot(freqs, abs(squeeze(freqresp(G(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [(m/s)/N]'); set(gca, 'XTickLabel',[]);
+
+ax2 = subplot(2, 1, 2);
+hold on;
+for i = 1:6
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2],'x');
+
+initDisturbances();
+
+initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
+
+load('mat/conf_simscape.mat');
+set_param(conf_simscape, 'StopTime', '3');
+
+sim('sim_nass_active_damping');
+
+save('./active_damping/mat/tomo_exp.mat', 'Ern', 't');
+
+figure;
+hold on;
+plot(t, Ern(:,1), 'DisplayName', '$\epsilon_{x}$')
+plot(t, Ern(:,2), 'DisplayName', '$\epsilon_{y}$')
+plot(t, Ern(:,3), 'DisplayName', '$\epsilon_{z}$')
+hold off;
+xlim([1,inf]);
+legend();
+xlabel('Time [s]'); ylabel('Position Error [m]');
+
+figure;
+hold on;
+plot(t, Ern(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
+plot(t, Ern(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
+plot(t, Ern(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
+hold off;
+xlim([1,inf]);
+legend();
+xlabel('Time [s]'); ylabel('Position Error [rad]');
diff --git a/active_damping/src/prepareTomographyExperiment.m b/active_damping/src/prepareTomographyExperiment.m
new file mode 100644
index 0000000..6fd73d1
--- /dev/null
+++ b/active_damping/src/prepareTomographyExperiment.m
@@ -0,0 +1,30 @@
+function [] = prepareTomographyExperiment(args)
+
+arguments
+    args.nass_actuator       char   {mustBeMember(args.nass_actuator,{'piezo', 'lorentz'})} = 'piezo'
+    args.sample_mass   (1,1) double {mustBeNumeric, mustBePositive} = 50
+    args.Ry_period     (1,1) double {mustBeNumeric, mustBePositive} = 1
+end
+
+initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+initializeAxisc();
+initializeMirror();
+
+initializeNanoHexapod('actuator', args.nass_actuator);
+initializeSample('mass', args.sample_mass);
+
+initializeReferences('Rz_type', 'rotating', 'Rz_period', args.Ry_period);
+
+K = tf(zeros(6));
+save('./mat/controllers.mat', 'K', '-append');
+K_ine = tf(zeros(6));
+save('./mat/controllers.mat', 'K_ine', '-append');
+K_iff = tf(zeros(6));
+save('./mat/controllers.mat', 'K_iff', '-append');
+K_dvf = tf(zeros(6));
+save('./mat/controllers.mat', 'K_dvf', '-append');
diff --git a/figs/dvf_open_loop.png b/figs/dvf_open_loop.png
new file mode 100644
index 0000000..29d0572
Binary files /dev/null and b/figs/dvf_open_loop.png differ
diff --git a/figs/dvf_plant.png b/figs/dvf_plant.png
index 48bd012..86a7490 100644
Binary files a/figs/dvf_plant.png and b/figs/dvf_plant.png differ
diff --git a/figs/iff_open_loop.png b/figs/iff_open_loop.png
index 35e05e7..66bcf5c 100644
Binary files a/figs/iff_open_loop.png and b/figs/iff_open_loop.png differ
diff --git a/figs/iff_plant.png b/figs/iff_plant.png
index 91fc7db..76b3107 100644
Binary files a/figs/iff_plant.png and b/figs/iff_plant.png differ
diff --git a/figs/ine_open_loop_gain.png b/figs/ine_open_loop_gain.png
new file mode 100644
index 0000000..fef2c82
Binary files /dev/null and b/figs/ine_open_loop_gain.png differ
diff --git a/figs/ine_plant.png b/figs/ine_plant.png
new file mode 100644
index 0000000..9f8fcc8
Binary files /dev/null and b/figs/ine_plant.png differ
diff --git a/figs/nass_act_damp_dvf_sim_tomo_rot.png b/figs/nass_act_damp_dvf_sim_tomo_rot.png
new file mode 100644
index 0000000..e4400a4
Binary files /dev/null and b/figs/nass_act_damp_dvf_sim_tomo_rot.png differ
diff --git a/figs/nass_act_damp_dvf_sim_tomo_trans.png b/figs/nass_act_damp_dvf_sim_tomo_trans.png
new file mode 100644
index 0000000..b93493e
Binary files /dev/null and b/figs/nass_act_damp_dvf_sim_tomo_trans.png differ
diff --git a/figs/nass_act_damp_iff_sim_tomo_rot.png b/figs/nass_act_damp_iff_sim_tomo_rot.png
new file mode 100644
index 0000000..336f3f6
Binary files /dev/null and b/figs/nass_act_damp_iff_sim_tomo_rot.png differ
diff --git a/figs/nass_act_damp_iff_sim_tomo_trans.png b/figs/nass_act_damp_iff_sim_tomo_trans.png
new file mode 100644
index 0000000..0c524ec
Binary files /dev/null and b/figs/nass_act_damp_iff_sim_tomo_trans.png differ
diff --git a/figs/nass_act_damp_ine_sim_tomo_rot.png b/figs/nass_act_damp_ine_sim_tomo_rot.png
new file mode 100644
index 0000000..c32e705
Binary files /dev/null and b/figs/nass_act_damp_ine_sim_tomo_rot.png differ
diff --git a/figs/nass_act_damp_ine_sim_tomo_trans.png b/figs/nass_act_damp_ine_sim_tomo_trans.png
new file mode 100644
index 0000000..5ed71b4
Binary files /dev/null and b/figs/nass_act_damp_ine_sim_tomo_trans.png differ
diff --git a/figs/nass_act_damp_undamped_sim_tomo_rot.png b/figs/nass_act_damp_undamped_sim_tomo_rot.png
new file mode 100644
index 0000000..abf2582
Binary files /dev/null and b/figs/nass_act_damp_undamped_sim_tomo_rot.png differ
diff --git a/figs/nass_act_damp_undamped_sim_tomo_trans.png b/figs/nass_act_damp_undamped_sim_tomo_trans.png
new file mode 100644
index 0000000..4cda0fe
Binary files /dev/null and b/figs/nass_act_damp_undamped_sim_tomo_trans.png differ
diff --git a/figs/nass_active_damping_iff_plant.png b/figs/nass_active_damping_iff_plant.png
new file mode 100644
index 0000000..9f34324
Binary files /dev/null and b/figs/nass_active_damping_iff_plant.png differ
diff --git a/figs/nass_active_damping_ine_plant.png b/figs/nass_active_damping_ine_plant.png
new file mode 100644
index 0000000..86a7490
Binary files /dev/null and b/figs/nass_active_damping_ine_plant.png differ
diff --git a/figs/nass_active_damping_inertial_plant.png b/figs/nass_active_damping_inertial_plant.png
new file mode 100644
index 0000000..481d8e9
Binary files /dev/null and b/figs/nass_active_damping_inertial_plant.png differ
diff --git a/figs/rmc_open_loop.png b/figs/rmc_open_loop.png
index 4633884..28449ce 100644
Binary files a/figs/rmc_open_loop.png and b/figs/rmc_open_loop.png differ
diff --git a/figs/rmc_plant.png b/figs/rmc_plant.png
index a5be88c..86a7490 100644
Binary files a/figs/rmc_plant.png and b/figs/rmc_plant.png differ
diff --git a/simscape_subsystems/index.org b/simscape_subsystems/index.org
index be6f4f0..66fa113 100644
--- a/simscape_subsystems/index.org
+++ b/simscape_subsystems/index.org
@@ -107,7 +107,7 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
 
       %% Low Pass Filter to filter out the references
       s = zpk('s');
-      w0 = 2*pi*100;
+      w0 = 2*pi*10;
       xi = 1;
       H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
 #+end_src
diff --git a/simscape_subsystems/nano_hexapod_F.slx b/simscape_subsystems/nano_hexapod_F.slx
index 8f23d37..f430125 100644
Binary files a/simscape_subsystems/nano_hexapod_F.slx and b/simscape_subsystems/nano_hexapod_F.slx differ
diff --git a/simscape_subsystems/nano_hexapod_leg.slx b/simscape_subsystems/nano_hexapod_leg.slx
index f1835be..03118bb 100644
Binary files a/simscape_subsystems/nano_hexapod_leg.slx and b/simscape_subsystems/nano_hexapod_leg.slx differ
diff --git a/src/initializeReferences.m b/src/initializeReferences.m
index f59c9ad..39d18ea 100644
--- a/src/initializeReferences.m
+++ b/src/initializeReferences.m
@@ -43,7 +43,7 @@ Tmax = args.Tmax;
 
 %% Low Pass Filter to filter out the references
 s = zpk('s');
-w0 = 2*pi*100;
+w0 = 2*pi*10;
 xi = 1;
 H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
 
