Reworked all computation / new CSS / export to PDF

.gitignore

@@ -35,3 +35,266 @@ codegen/
 
 # Octave session info
 octave-workspace
+
+.auctex-auto/
+.log/
+
+## Core latex/pdflatex auxiliary files:
+*.aux
+*.lof
+*.log
+*.lot
+*.fls
+*.out
+*.toc
+*.fmt
+*.fot
+*.cb
+*.cb2
+.*.lb
+
+## Intermediate documents:
+*.dvi
+*.xdv
+*-converted-to.*
+# these rules might exclude image files for figures etc.
+# *.ps
+# *.eps
+# *.pdf
+
+## Generated if empty string is given at "Please type another file name for output:"
+.pdf
+
+## Bibliography auxiliary files (bibtex/biblatex/biber):
+*.bbl
+*.bcf
+*.blg
+*-blx.aux
+*-blx.bib
+*.run.xml
+
+## Build tool auxiliary files:
+*.fdb_latexmk
+*.synctex
+*.synctex(busy)
+*.synctex.gz
+*.synctex.gz(busy)
+*.pdfsync
+
+## Build tool directories for auxiliary files
+# latexrun
+latex.out/
+
+## Auxiliary and intermediate files from other packages:
+# algorithms
+*.alg
+*.loa
+
+# achemso
+acs-*.bib
+
+# amsthm
+*.thm
+
+# beamer
+*.nav
+*.pre
+*.snm
+*.vrb
+
+# changes
+*.soc
+
+# comment
+*.cut
+
+# cprotect
+*.cpt
+
+# elsarticle (documentclass of Elsevier journals)
+*.spl
+
+# endnotes
+*.ent
+
+# fixme
+*.lox
+
+# feynmf/feynmp
+*.mf
+*.mp
+*.t[1-9]
+*.t[1-9][0-9]
+*.tfm
+
+#(r)(e)ledmac/(r)(e)ledpar
+*.end
+*.?end
+*.[1-9]
+*.[1-9][0-9]
+*.[1-9][0-9][0-9]
+*.[1-9]R
+*.[1-9][0-9]R
+*.[1-9][0-9][0-9]R
+*.eledsec[1-9]
+*.eledsec[1-9]R
+*.eledsec[1-9][0-9]
+*.eledsec[1-9][0-9]R
+*.eledsec[1-9][0-9][0-9]
+*.eledsec[1-9][0-9][0-9]R
+
+# glossaries
+*.acn
+*.acr
+*.glg
+*.glo
+*.gls
+*.glsdefs
+
+# gnuplottex
+*-gnuplottex-*
+
+# gregoriotex
+*.gaux
+*.gtex
+
+# htlatex
+*.4ct
+*.4tc
+*.idv
+*.lg
+*.trc
+*.xref
+
+# hyperref
+*.brf
+
+# knitr
+*-concordance.tex
+# TODO Comment the next line if you want to keep your tikz graphics files
+*.tikz
+*-tikzDictionary
+
+# listings
+*.lol
+
+# makeidx
+*.idx
+*.ilg
+*.ind
+*.ist
+
+# minitoc
+*.maf
+*.mlf
+*.mlt
+*.mtc[0-9]*
+*.slf[0-9]*
+*.slt[0-9]*
+*.stc[0-9]*
+
+# minted
+_minted*
+*.pyg
+
+# morewrites
+*.mw
+
+# nomencl
+*.nlg
+*.nlo
+*.nls
+
+# pax
+*.pax
+
+# pdfpcnotes
+*.pdfpc
+
+# sagetex
+*.sagetex.sage
+*.sagetex.py
+*.sagetex.scmd
+
+# scrwfile
+*.wrt
+
+# sympy
+*.sout
+*.sympy
+sympy-plots-for-*.tex/
+
+# pdfcomment
+*.upa
+*.upb
+
+# pythontex
+*.pytxcode
+pythontex-files-*/
+
+# tcolorbox
+*.listing
+
+# thmtools
+*.loe
+
+# TikZ & PGF
+*.dpth
+*.md5
+*.auxlock
+
+# todonotes
+*.tdo
+
+# vhistory
+*.hst
+*.ver
+
+# easy-todo
+*.lod
+
+# xcolor
+*.xcp
+
+# xmpincl
+*.xmpi
+
+# xindy
+*.xdy
+
+# xypic precompiled matrices
+*.xyc
+
+# endfloat
+*.ttt
+*.fff
+
+# Latexian
+TSWLatexianTemp*
+
+## Editors:
+# WinEdt
+*.bak
+*.sav
+
+# Texpad
+.texpadtmp
+
+# LyX
+*.lyx~
+
+# Kile
+*.backup
+
+# KBibTeX
+*~[0-9]*
+
+# auto folder when using emacs and auctex
+./auto/*
+*.el
+
+# expex forward references with \gathertags
+*-tags.tex
+
+# standalone packages
+*.sta

css/custom.css

@@ -0,0 +1,49 @@
+.figure p{
+    text-align: center;
+}
+
+.figure img{
+    max-width:100%;
+    display: block;
+    margin: auto;
+}
+
+table {
+    margin-left: auto;
+    margin-right: auto;
+}
+
+.org-src-container > pre.src:before {
+  display: inline;
+  position: absolute;
+  color: #808080;
+  background-color: white;
+  top: -10px;
+  left: 10px;
+  padding: 0px 4px;
+  border: 1px solid #d0d0d0;
+  font-size: 80%;
+}
+.org-src-container > pre {
+  margin-top: 1.5em;
+  position: relative;
+  overflow: visible;
+}
+.org-src-container > pre > code.src:before {
+  display: inline;
+  position: absolute;
+  color: #808080;
+  background-color: white;
+  top: -10px;
+  left: 10px;
+  padding: 0px 4px;
+  border: 1px solid #d0d0d0;
+  font-size: 80%;
+}
+.org-src-container > pre.src-emacs-lisp:before { content: 'Emacs Lisp'; }
+.org-src-container > pre.src-elisp:before { content: 'Emacs Lisp'; }
+.org-src-container > pre.src-sh:before { content: 'shell'; }
+.org-src-container > pre.src-bash:before { content: 'bash'; }
+.org-src-container > pre.src-org:before { content: 'Org mode'; }
+.org-src-container > pre.src-python:before { content: 'Python'; }
+.org-src-container > pre.src-matlab:before { content: 'Matlab'; }

css/readtheorg.css

@@ -513,7 +513,7 @@ legend{
     padding:0;
     white-space:normal}
 
-.fa:before,#content .admonition-title:before,#content h1 .headerlink:before,#content h2 .headerlink:before,#content h3 .headerlink:before,#content h4 .headerlink:before,#content h5 .headerlink:before,#content h6 .headerlink:before,#content dl dt .headerlink:before,.icon:before,.wy-dropdown .caret:before,.wy-inline-validate.wy-inline-validate-success .wy-input-context:before,.wy-inline-validate.wy-inline-validate-danger .wy-input-context:before,.wy-inline-validate.wy-inline-validate-warning .wy-input-context:before,.wy-inline-validate.wy-inline-validate-info .wy-input-context:before,.wy-alert,#content .note,#content .attention,#content .caution,#content .danger,#content .error,#content .hint,#content .important,#content .tip,#content .warning,#content .seealso,#content .admonitiontodo,.btn,input[type="text"],input[type="password"],input[type="email"],input[type="url"],input[type="date"],input[type="month"],input[type="time"],input[type="datetime"],input[type="datetime-local"],input[type="week"],input[type="number"],input[type="search"],input[type="tel"],input[type="color"],select,textarea,#table-of-contents li.on a,#table-of-contents li.current>a,.wy-side-nav-search>a,.wy-side-nav-search .wy-dropdown>a,.wy-nav-top a{
+.fa:before,#content .admonition-title:before,#content h1 .headerlink:before,#content h2 .headerlink:before,#content h3 .headerlink:before,#content h4 .headerlink:before,#content h5 .headerlink:before,#content h6 .headerlink:before,#content dl dt .headerlink:before,.icon:before,.wy-dropdown .caret:before,.wy-inline-validate.wy-inline-validate-success .wy-input-context:before,.wy-inline-validate.wy-inline-validate-danger .wy-input-context:before,.wy-inline-validate.wy-inline-validate-warning .wy-input-context:before,.wy-inline-validate.wy-inline-validate-info .wy-input-context:before,.wy-alert,#content .note,#content .attention,#content .caution,#content .danger,#content .error,#content .summary,#content .hint,#content .important,#content .tip,#content .warning,#content .question,#content .seealso,#content .admonitiontodo,.btn,input[type="text"],input[type="password"],input[type="email"],input[type="url"],input[type="date"],input[type="month"],input[type="time"],input[type="datetime"],input[type="datetime-local"],input[type="week"],input[type="number"],input[type="search"],input[type="tel"],input[type="color"],select,textarea,#table-of-contents li.on a,#table-of-contents li.current>a,.wy-side-nav-search>a,.wy-side-nav-search .wy-dropdown>a,.wy-nav-top a{
     -webkit-font-smoothing:antialiased}
 
 /*!
@@ -576,7 +576,7 @@ a .fa,a #content .admonition-title,#content a .admonition-title{
 .nav #content .admonition-title,#content .nav .admonition-title,.nav .icon{
     display:inline}
 
-.wy-alert,#content .note,#content .attention,#content .caution,#content .danger,#content .error,#content .hint,#content .important,#content .tip,#content .warning,#content .seealso,#content .admonitiontodo{
+.wy-alert,#content .note,#content .attention,#content .caution,#content .danger,#content .error,#content .summary,#content .hint,#content .important,#content .tip,#content .warning,#content .question,#content .seealso,#content .admonitiontodo{
     padding:12px;
     line-height:24px;
     margin-bottom:24px;
@@ -596,32 +596,45 @@ a .fa,a #content .admonition-title,#content a .admonition-title{
 #content .danger,#content .error{
     background:#fdf3f2}
 
-.wy-alert.wy-alert-warning,#content .wy-alert-warning.note,#content .attention,#content .caution,#content .wy-alert-warning.danger,#content .wy-alert-warning.error,#content .wy-alert-warning.hint,#content .wy-alert-warning.important,#content .wy-alert-warning.tip,#content .warning,#content .wy-alert-warning.seealso,#content .admonitiontodo{
+.wy-alert.wy-alert-warning,#content .wy-alert-warning.note,#content .attention,#content .caution,#content .wy-alert-warning.danger,#content .wy-alert-warning.error,#content .wy-alert-warning.summary,#content .wy-alert-warning.hint,#content .wy-alert-warning.important,#content .wy-alert-warning.tip,#content .warning,#content .wy-alert-warning.seealso,#content .admonitiontodo{
     background:#ffedcc}
 
-#content .admonition-title.note:before, #content .admonition-title.seealso:before,
-#content .admonition-title.warning:before, #content .admonition-title.caution:before,
+#content .admonition-title.warning:before,
+#content .admonition-title.caution:before,
 #content .admonition-title.attention:before,
-#content .admonition-title.tip:before, #content .admonition-title.hint:before,
 #content .admonition-title.important:before,
-#content .admonition-title.error:before, #content .admonition-title.danger:before{
+#content .admonition-title.error:before,
+#content .admonition-title.danger:before{
     font-family:FontAwesome;
     content: "";}
 
-#content .note,#content .seealso{
+#content .admonition-title.question:before{
+    font-family:FontAwesome;
+    content: "";}
+
+#content .admonition-title.note:before,
+#content .admonition-title.seealso:before,
+#content .admonition-title.tip:before,
+#content .admonition-title.summary:before,
+#content .admonition-title.hint:before{
+    font-family:FontAwesome;
+    content: "";}
+
+
+#content .note,#content .question,#content .seealso{
     background:#e7f2fa}
 
-.wy-alert p:last-child,#content .note p:last-child,#content .attention p:last-child,#content .caution p:last-child,#content .danger p:last-child,#content .error p:last-child,#content .hint p:last-child,#content .important p:last-child,#content .tip p:last-child,#content .warning p:last-child,#content .seealso p:last-child,#content .admonitiontodo p:last-child{
+.wy-alert p:last-child,#content .note p:last-child,#content .attention p:last-child,#content .caution p:last-child,#content .danger p:last-child,#content .error p:last-child,#content .summary p:last-child,#content .hint p:last-child,#content .important p:last-child,#content .tip p:last-child,#content .warning p:last-child,#content .question p:last-child,#content .seealso p:last-child,#content .admonitiontodo p:last-child{
     margin-bottom:0}
 
-#content .admonition-title.tip,#content .admonition-title.important,#content .admonition-title.hint{
+#content .admonition-title.tip,#content .admonition-title.important,#content .admonition-title.summary,#content .admonition-title.hint{
     line-height: 1;
     background:#1abc9c}
 
-#content .important,#content .tip,#content .hint{
+#content .important,#content .tip,#content .summary,#content .hint{
     background:#dbfaf4}
 
-#content .admonition-title.note,#content .admonition-title.seealso{
+#content .admonition-title.note,#content .admonition-title.question,#content .admonition-title.seealso{
     line-height: 1;
     background:#6ab0de}
 
@@ -938,7 +951,7 @@ footer p{
     font-style:italic;
 }
 
-#content .note .last,#content .attention .last,#content .caution .last,#content .danger .last,#content .error .last,#content .hint .last,#content .important .last,#content .tip .last,#content .warning .last,#content .seealso .last,#content .admonitiontodo .last{
+#content .note .last,#content .attention .last,#content .caution .last,#content .danger .last,#content .error .last,#content .hint .summary,#content .hint .last,#content .important .last,#content .tip .last,#content .warning .last,#content .question .last,#content .seealso .last,#content .admonitiontodo .last{
     margin-bottom:0}
 
 #content .admonition-title:before{

js/jquery.stickytableheaders.min.js

@@ -1 +0,0 @@
-!function(a,b){"use strict";function c(c,g){var h=this;h.$el=a(c),h.el=c,h.id=e++,h.$window=a(b),h.$document=a(document),h.$el.bind("destroyed",a.proxy(h.teardown,h)),h.$clonedHeader=null,h.$originalHeader=null,h.isSticky=!1,h.hasBeenSticky=!1,h.leftOffset=null,h.topOffset=null,h.init=function(){h.$el.each(function(){var b=a(this);b.css("padding",0),h.$originalHeader=a("thead:first",this),h.$clonedHeader=h.$originalHeader.clone(),b.trigger("clonedHeader."+d,[h.$clonedHeader]),h.$clonedHeader.addClass("tableFloatingHeader"),h.$clonedHeader.css("display","none"),h.$originalHeader.addClass("tableFloatingHeaderOriginal"),h.$originalHeader.after(h.$clonedHeader),h.$printStyle=a('<style type="text/css" media="print">.tableFloatingHeader{display:none !important;}.tableFloatingHeaderOriginal{position:static !important;}</style>'),a("head").append(h.$printStyle)}),h.setOptions(g),h.updateWidth(),h.toggleHeaders(),h.bind()},h.destroy=function(){h.$el.unbind("destroyed",h.teardown),h.teardown()},h.teardown=function(){h.isSticky&&h.$originalHeader.css("position","static"),a.removeData(h.el,"plugin_"+d),h.unbind(),h.$clonedHeader.remove(),h.$originalHeader.removeClass("tableFloatingHeaderOriginal"),h.$originalHeader.css("visibility","visible"),h.$printStyle.remove(),h.el=null,h.$el=null},h.bind=function(){h.$scrollableArea.on("scroll."+d,h.toggleHeaders),h.isWindowScrolling||(h.$window.on("scroll."+d+h.id,h.setPositionValues),h.$window.on("resize."+d+h.id,h.toggleHeaders)),h.$scrollableArea.on("resize."+d,h.toggleHeaders),h.$scrollableArea.on("resize."+d,h.updateWidth)},h.unbind=function(){h.$scrollableArea.off("."+d,h.toggleHeaders),h.isWindowScrolling||(h.$window.off("."+d+h.id,h.setPositionValues),h.$window.off("."+d+h.id,h.toggleHeaders)),h.$scrollableArea.off("."+d,h.updateWidth)},h.toggleHeaders=function(){h.$el&&h.$el.each(function(){var b,c=a(this),d=h.isWindowScrolling?isNaN(h.options.fixedOffset)?h.options.fixedOffset.outerHeight():h.options.fixedOffset:h.$scrollableArea.offset().top+(isNaN(h.options.fixedOffset)?0:h.options.fixedOffset),e=c.offset(),f=h.$scrollableArea.scrollTop()+d,g=h.$scrollableArea.scrollLeft(),i=h.isWindowScrolling?f>e.top:d>e.top,j=(h.isWindowScrolling?f:0)<e.top+c.height()-h.$clonedHeader.height()-(h.isWindowScrolling?0:d);i&&j?(b=e.left-g+h.options.leftOffset,h.$originalHeader.css({position:"fixed","margin-top":h.options.marginTop,left:b,"z-index":3}),h.leftOffset=b,h.topOffset=d,h.$clonedHeader.css("display",""),h.isSticky||(h.isSticky=!0,h.updateWidth()),h.setPositionValues()):h.isSticky&&(h.$originalHeader.css("position","static"),h.$clonedHeader.css("display","none"),h.isSticky=!1,h.resetWidth(a("td,th",h.$clonedHeader),a("td,th",h.$originalHeader)))})},h.setPositionValues=function(){var a=h.$window.scrollTop(),b=h.$window.scrollLeft();!h.isSticky||0>a||a+h.$window.height()>h.$document.height()||0>b||b+h.$window.width()>h.$document.width()||h.$originalHeader.css({top:h.topOffset-(h.isWindowScrolling?0:a),left:h.leftOffset-(h.isWindowScrolling?0:b)})},h.updateWidth=function(){if(h.isSticky){h.$originalHeaderCells||(h.$originalHeaderCells=a("th,td",h.$originalHeader)),h.$clonedHeaderCells||(h.$clonedHeaderCells=a("th,td",h.$clonedHeader));var b=h.getWidth(h.$clonedHeaderCells);h.setWidth(b,h.$clonedHeaderCells,h.$originalHeaderCells),h.$originalHeader.css("width",h.$clonedHeader.width())}},h.getWidth=function(c){var d=[];return c.each(function(c){var e,f=a(this);if("border-box"===f.css("box-sizing"))e=f[0].getBoundingClientRect().width;else{var g=a("th",h.$originalHeader);if("collapse"===g.css("border-collapse"))if(b.getComputedStyle)e=parseFloat(b.getComputedStyle(this,null).width);else{var i=parseFloat(f.css("padding-left")),j=parseFloat(f.css("padding-right")),k=parseFloat(f.css("border-width"));e=f.outerWidth()-i-j-k}else e=f.width()}d[c]=e}),d},h.setWidth=function(a,b,c){b.each(function(b){var d=a[b];c.eq(b).css({"min-width":d,"max-width":d})})},h.resetWidth=function(b,c){b.each(function(b){var d=a(this);c.eq(b).css({"min-width":d.css("min-width"),"max-width":d.css("max-width")})})},h.setOptions=function(c){h.options=a.extend({},f,c),h.$scrollableArea=a(h.options.scrollableArea),h.isWindowScrolling=h.$scrollableArea[0]===b},h.updateOptions=function(a){h.setOptions(a),h.unbind(),h.bind(),h.updateWidth(),h.toggleHeaders()},h.init()}var d="stickyTableHeaders",e=0,f={fixedOffset:0,leftOffset:0,marginTop:0,scrollableArea:b};a.fn[d]=function(b){return this.each(function(){var e=a.data(this,"plugin_"+d);e?"string"==typeof b?e[b].apply(e):e.updateOptions(b):"destroy"!==b&&a.data(this,"plugin_"+d,new c(this,b))})}}(jQuery,window);

js/readtheorg.js

@@ -9,6 +9,8 @@ $(function() {
     $('.hint').before("<p class='admonition-title hint'>Hint</p>");
     $('.error').before("<p class='admonition-title error'>Error</p>");
     $('.danger').before("<p class='admonition-title danger'>Danger</p>");
+    $('.question').before("<p class='admonition-title question'>Question</p>");
+    $('.summary').before("<p class='admonition-title hint'>Summary</p>");
 });
 
 $( document ).ready(function() {

matlab/first_identification.m

@@ -0,0 +1,423 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+addpath('./mat/');
+
+% Load Data
+% The data is loaded in the Matlab workspace.
+
+id_ol = load('identification_noise_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
+
+
+
+% Then, any offset is removed.
+
+id_ol.d      = detrend(id_ol.d, 0);
+id_ol.acc_1  = detrend(id_ol.acc_1, 0);
+id_ol.acc_2  = detrend(id_ol.acc_2, 0);
+id_ol.geo_1  = detrend(id_ol.geo_1, 0);
+id_ol.geo_2  = detrend(id_ol.geo_2, 0);
+id_ol.f_meas = detrend(id_ol.f_meas, 0);
+id_ol.u      = detrend(id_ol.u, 0);
+
+% Excitation Signal
+% The generated voltage used to excite the system is a white noise and can be seen in Figure [[fig:excitation_signal_first_identification]].
+
+
+figure;
+plot(id_ol.t, id_ol.u)
+xlabel('Time [s]'); ylabel('Voltage [V]');
+
+% Identified Plant
+% The transfer function from the excitation voltage to the mass displacement and to the force sensor stack voltage are identified using the =tfestimate= command.
+
+
+Ts = id_ol.t(2) - id_ol.t(1);
+win = hann(ceil(10/Ts));
+
+[tf_fmeas_est, f] = tfestimate(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts); % [V/V]
+[tf_G_ol_est,  ~] = tfestimate(id_ol.u, id_ol.d, win, [], [], 1/Ts); % [m/V]
+
+
+
+% The bode plots of the obtained dynamics are shown in Figures [[fig:force_sensor_bode_plot]] and [[fig:displacement_sensor_bode_plot]].
+
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(tf_fmeas_est), '-')
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(tf_fmeas_est), '-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2], 'x');
+xlim([1, 1e3]);
+
+
+
+% #+name: fig:force_sensor_bode_plot
+% #+caption: Bode plot of the dynamics from excitation voltage to measured force sensor stack voltage
+% #+RESULTS:
+% [[file:figs/force_sensor_bode_plot.png]]
+
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(tf_G_ol_est), '-')
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(tf_G_ol_est), '-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2], 'x');
+xlim([1, 1e3]);
+
+% Simscape Model - Comparison
+% A simscape model representing the test-bench has been developed.
+% The same transfer functions as the one identified using the test-bench can be obtained thanks to the simscape model.
+
+% They are compared in Figure [[fig:simscape_comp_iff_plant]] and [[fig:simscape_comp_disp_plant]].
+% It is shown that there is a good agreement between the model and the experiment.
+
+
+load('piezo_amplified_3d.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');
+
+m = 10;
+Kiff = tf(0);
+
+%% Name of the Simulink File
+mdl = 'sensor_fusion_test_bench_simscape';
+
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/Fd'], 1, 'openinput');  io_i = io_i + 1; % External Vertical Force [N]
+io(io_i) = linio([mdl, '/w'],  1, 'openinput');  io_i = io_i + 1; % Base Motion [m]
+io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1; % Actuator Voltage [V]
+io(io_i) = linio([mdl, '/Interferometer'],  1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
+io(io_i) = linio([mdl, '/Voltage_Conditioner'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor [V]
+
+options = linearizeOptions('SampleTime', 1e-4);
+G = linearize(mdl, io, options);
+
+G.InputName = {'Fd', 'w', 'Va'};
+G.OutputName = {'y', 'Vs'};
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(tf_fmeas_est), 'DisplayName', 'Identification')
+plot(f, abs(squeeze(freqresp(G('Vs', 'Va'), f, 'Hz'))), 'DisplayName', 'Simscape Model')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
+hold off;
+ylim([1e-1, 1e3]);
+legend('location', 'northwest');
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(tf_fmeas_est))
+plot(f, 180/pi*angle(squeeze(freqresp(G('Vs', 'Va'), f, 'Hz'))))
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2], 'x');
+xlim([1, 5e3]);
+
+
+
+% #+name: fig:simscape_comp_iff_plant
+% #+caption: Comparison of the dynamics from excitation voltage to measured force sensor stack voltage - Identified dynamics and Simscape Model
+% #+RESULTS:
+% [[file:figs/simscape_comp_iff_plant.png]]
+
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(tf_G_ol_est), 'DisplayName', 'Identification')
+plot(f, abs(squeeze(freqresp(G('y', 'Va'), f, 'Hz'))), 'DisplayName', 'Simscape Model')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+hold off;
+ylim([1e-8, 1e-3]);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(tf_G_ol_est))
+plot(f, 180/pi*angle(squeeze(freqresp(G('y', 'Va'), f, 'Hz'))))
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2], 'x');
+xlim([1, 5e3]);
+
+% Integral Force Feedback
+% The force sensor stack can be used to damp the system.
+% This makes the system easier to excite properly without too much amplification near resonances.
+
+% This is done thanks to the integral force feedback control architecture.
+
+% The force sensor stack signal is integrated (or rather low pass filtered) and fed back to the force sensor stacks.
+
+% The low pass filter used as the controller is defined below:
+
+Kiff = 102/(s + 2*pi*2);
+
+
+
+% The integral force feedback control strategy is applied to the simscape model as well as to the real test bench.
+
+
+%% Name of the Simulink File
+mdl = 'sensor_fusion_test_bench_simscape';
+
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/Fd'], 1, 'openinput');  io_i = io_i + 1; % External Vertical Force [N]
+io(io_i) = linio([mdl, '/w'],  1, 'openinput');  io_i = io_i + 1; % Base Motion [m]
+io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1; % Actuator Voltage [V]
+io(io_i) = linio([mdl, '/Interferometer'],  1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
+io(io_i) = linio([mdl, '/Voltage_Conditioner'], 1, 'output'); io_i = io_i + 1; % Force Sensor [V]
+
+options = linearizeOptions('SampleTime', 1e-4);
+G_cl = linearize(mdl, io, options);
+
+G_cl.InputName = {'Fd', 'w', 'Va'};
+G_cl.OutputName = {'y', 'Vs'};
+
+
+
+% The damped system is then identified again using a noise excitation.
+
+% The data is loaded into Matlab and any offset is removed.
+
+id_cl = load('identification_noise_iff_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
+
+id_cl.d      = detrend(id_cl.d, 0);
+id_cl.acc_1  = detrend(id_cl.acc_1, 0);
+id_cl.acc_2  = detrend(id_cl.acc_2, 0);
+id_cl.geo_1  = detrend(id_cl.geo_1, 0);
+id_cl.geo_2  = detrend(id_cl.geo_2, 0);
+id_cl.f_meas = detrend(id_cl.f_meas, 0);
+id_cl.u      = detrend(id_cl.u, 0);
+
+
+
+% The transfer functions are estimated using =tfestimate=.
+
+[tf_G_cl_est, ~] = tfestimate(id_cl.u, id_cl.d, win, [], [], 1/Ts);
+[co_G_cl_est, ~] = mscohere(  id_cl.u, id_cl.d, win, [], [], 1/Ts);
+
+
+
+% The dynamics from driving voltage to the measured displacement are compared both in the open-loop and IFF case, and for the test-bench experimental identification and for the Simscape model in Figure [[fig:iff_ol_cl_identified_simscape_comp]].
+% This shows that the Integral Force Feedback architecture effectively damps the first resonance of the system.
+
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+set(gca, 'ColorOrderIndex', 1);
+plot(f, abs(tf_G_ol_est), '-', 'DisplayName', 'OL - Ident.')
+set(gca, 'ColorOrderIndex', 1);
+plot(f, abs(squeeze(freqresp(G('y', 'Va'), f, 'Hz'))), '--', 'DisplayName', 'OL - Simscape')
+set(gca, 'ColorOrderIndex', 2);
+plot(f, abs(tf_G_cl_est), '-', 'DisplayName', 'CL - Ident.')
+set(gca, 'ColorOrderIndex', 2);
+plot(f, abs(squeeze(freqresp(G_cl('y', 'Va'), f, 'Hz'))), '--', 'DisplayName', 'CL - Simscape')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+legend('location', 'northeast');
+hold off;
+ylim([1e-7, 1e-3]);
+
+ax2 = nexttile;
+hold on;
+set(gca, 'ColorOrderIndex', 1);
+plot(f, 180/pi*angle(tf_G_ol_est), '-')
+set(gca, 'ColorOrderIndex', 1);
+plot(f, 180/pi*angle(squeeze(freqresp(G('y', 'Va'), f, 'Hz'))), '--')
+set(gca, 'ColorOrderIndex', 2);
+plot(f, 180/pi*angle(tf_G_cl_est), '-')
+set(gca, 'ColorOrderIndex', 2);
+plot(f, 180/pi*angle(squeeze(freqresp(G_cl('y', 'Va'), f, 'Hz'))), '--')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2], 'x');
+xlim([1, 5e3]);
+
+% Inertial Sensors
+% In order to estimate the dynamics of the inertial sensor (the transfer function from the "absolute" displacement to the measured voltage), the following experiment can be performed:
+% - The mass is excited such that is relative displacement as measured by the interferometer is much larger that the ground "absolute" motion.
+% - The transfer function from the measured displacement by the interferometer to the measured voltage generated by the inertial sensors can be estimated.
+
+% The first point is quite important in order to have a good coherence between the interferometer measurement and the inertial sensor measurement.
+
+% Here, a first identification is performed were the excitation signal is a white noise.
+
+
+% As usual, the data is loaded and any offset is removed.
+
+id = load('identification_noise_opt_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
+
+id.d = detrend(id.d, 0);
+id.acc_1 = detrend(id.acc_1, 0);
+id.acc_2 = detrend(id.acc_2, 0);
+id.geo_1 = detrend(id.geo_1, 0);
+id.geo_2 = detrend(id.geo_2, 0);
+id.f_meas = detrend(id.f_meas, 0);
+
+
+
+% Then the transfer functions from the measured displacement by the interferometer to the generated voltage of the inertial sensors are computed..
+
+Ts = id.t(2) - id.t(1);
+win = hann(ceil(10/Ts));
+
+[tf_acc1_est, f] = tfestimate(id.d, id.acc_1, win, [], [], 1/Ts);
+[co_acc1_est, ~] = mscohere(  id.d, id.acc_1, win, [], [], 1/Ts);
+[tf_acc2_est, ~] = tfestimate(id.d, id.acc_2, win, [], [], 1/Ts);
+[co_acc2_est, ~] = mscohere(  id.d, id.acc_2, win, [], [], 1/Ts);
+
+[tf_geo1_est, ~] = tfestimate(id.d, id.geo_1, win, [], [], 1/Ts);
+[co_geo1_est, ~] = mscohere(  id.d, id.geo_1, win, [], [], 1/Ts);
+[tf_geo2_est, ~] = tfestimate(id.d, id.geo_2, win, [], [], 1/Ts);
+[co_geo2_est, ~] = mscohere(  id.d, id.geo_2, win, [], [], 1/Ts);
+
+
+
+% The same transfer functions are estimated using the Simscape model.
+
+
+m = 10;
+Kiff = tf(0);
+
+%% Name of the Simulink File
+mdl = 'sensor_fusion_test_bench_simscape';
+
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1; % Actuator Voltage [V]
+io(io_i) = linio([mdl, '/Interferometer'],  1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
+io(io_i) = linio([mdl, '/Vertical_Accelerometer_1'], 1, 'openoutput'); io_i = io_i + 1; % Accelerometer [V]
+io(io_i) = linio([mdl, '/Voltage_Ampl_geo_1'], 1, 'openoutput'); io_i = io_i + 1; % Geophone [V]
+
+options = linearizeOptions('SampleTime', 1e-4);
+G = linearize(mdl, io, options);
+
+G.InputName = {'Va'};
+G.OutputName = {'y', 'a', 'v'};
+
+G_acc = G('a', 'Va')*inv(G('y', 'Va')); % [V/m]
+G_geo = G('v', 'Va')*inv(G('y', 'Va')); % [V/m]
+
+
+
+% The obtained dynamics of the accelerometer are compared in Figure [[fig:comp_dynamics_accelerometer]] while the one of the geophones are compared in Figure [[fig:comp_dynamics_geophone]].
+
+
+freqs = logspace(-1, 4, 1000)';
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(tf_acc1_est./(1i*2*pi*f).^2), '.')
+plot(f, abs(tf_acc2_est./(1i*2*pi*f).^2), '.')
+plot(freqs, abs(squeeze(freqresp(G_acc, freqs, 'Hz'))./(1i*2*pi*freqs).^2), 'k-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude $\left[\frac{V}{m/s^2}\right]$');  set(gca, 'XTickLabel',[]);
+hold off;
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(tf_acc1_est./(1i*2*pi*f).^2), '.')
+plot(f, 180/pi*angle(tf_acc2_est./(1i*2*pi*f).^2), '.')
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_acc, freqs, 'Hz'))./(1i*2*pi*freqs).^2), 'k-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2], 'x');
+xlim([2, 2e3]);
+
+
+
+% #+name: fig:comp_dynamics_accelerometer
+% #+caption: Comparison of the measured accelerometer dynamics
+% #+RESULTS:
+% [[file:figs/comp_dynamics_accelerometer.png]]
+
+
+freqs = logspace(-1, 4, 1000)';
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(tf_geo1_est./(1i*2*pi*f)), '.')
+plot(f, abs(tf_geo2_est./(1i*2*pi*f)), '.')
+plot(freqs, abs(squeeze(freqresp(G_geo, freqs, 'Hz'))./(1i*2*pi*freqs)), 'k-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude $\left[\frac{V}{m/s}\right]$');  set(gca, 'XTickLabel',[]);
+hold off;
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(tf_geo1_est./(1i*2*pi*f)), '.')
+plot(f, 180/pi*angle(tf_geo2_est./(1i*2*pi*f)), '.')
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_geo, freqs, 'Hz'))./(1i*2*pi*freqs)), 'k-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2], 'x');
+xlim([0.5, 2e3]);

matlab/inertial_sensor_dynamics.m

@@ -0,0 +1,200 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+addpath('./mat/');
+
+% Load Data
+% Both the measurement data during the identification test and during an "huddle test" are loaded.
+
+id = load('identification_noise_opt_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
+ht = load('huddle_test.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
+
+ht.d = detrend(ht.d, 0);
+ht.acc_1 = detrend(ht.acc_1, 0);
+ht.acc_2 = detrend(ht.acc_2, 0);
+ht.geo_1 = detrend(ht.geo_1, 0);
+ht.geo_2 = detrend(ht.geo_2, 0);
+ht.f_meas = detrend(ht.f_meas, 0);
+
+id.d = detrend(id.d, 0);
+id.acc_1 = detrend(id.acc_1, 0);
+id.acc_2 = detrend(id.acc_2, 0);
+id.geo_1 = detrend(id.geo_1, 0);
+id.geo_2 = detrend(id.geo_2, 0);
+id.f_meas = detrend(id.f_meas, 0);
+
+% Compare PSD during Huddle and and during identification
+% The Power Spectral Density of the measured motion during the huddle test and during the identification test are compared in Figures [[fig:comp_psd_huddle_test_identification_acc]] and [[fig:comp_psd_huddle_test_identification_geo]].
+
+
+Ts = ht.t(2) - ht.t(1);
+win = hann(ceil(10/Ts));
+
+[p_id_d, f] = pwelch(id.d, win, [], [], 1/Ts);
+[p_id_acc1, ~] = pwelch(id.acc_1, win, [], [], 1/Ts);
+[p_id_acc2, ~] = pwelch(id.acc_2, win, [], [], 1/Ts);
+[p_id_geo1, ~] = pwelch(id.geo_1, win, [], [], 1/Ts);
+[p_id_geo2, ~] = pwelch(id.geo_2, win, [], [], 1/Ts);
+
+[p_ht_d, ~] = pwelch(ht.d, win, [], [], 1/Ts);
+[p_ht_acc1, ~] = pwelch(ht.acc_1, win, [], [], 1/Ts);
+[p_ht_acc2, ~] = pwelch(ht.acc_2, win, [], [], 1/Ts);
+[p_ht_geo1, ~] = pwelch(ht.geo_1, win, [], [], 1/Ts);
+[p_ht_geo2, ~] = pwelch(ht.geo_2, win, [], [], 1/Ts);
+[p_ht_fmeas, ~] = pwelch(ht.f_meas, win, [], [], 1/Ts);
+
+figure;
+hold on;
+set(gca, 'ColorOrderIndex', 1);
+plot(f, p_ht_acc1, 'DisplayName', 'Huddle Test');
+set(gca, 'ColorOrderIndex', 1);
+plot(f, p_ht_acc2, 'HandleVisibility',  'off');
+set(gca, 'ColorOrderIndex', 2);
+plot(f, p_id_acc1, 'DisplayName', 'Identification Test');
+set(gca, 'ColorOrderIndex', 2);
+plot(f, p_id_acc2, 'HandleVisibility',  'off');
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
+title('Huddle Test - Accelerometers')
+legend('location', 'northwest');
+xlim([5e-1, 5e3]); ylim([1e-10, 1e-2])
+
+
+
+% #+name: fig:comp_psd_huddle_test_identification_acc
+% #+caption: Comparison of the PSD of the measured motion during the Huddle test and during the identification
+% #+RESULTS:
+% [[file:figs/comp_psd_huddle_test_identification_acc.png]]
+
+
+figure;
+hold on;
+set(gca, 'ColorOrderIndex', 1);
+plot(f, p_ht_geo1, 'DisplayName', 'Huddle Test');
+set(gca, 'ColorOrderIndex', 1);
+plot(f, p_ht_geo2, 'HandleVisibility',  'off');
+set(gca, 'ColorOrderIndex', 2);
+plot(f, p_id_geo1, 'DisplayName', 'Identification Test');
+set(gca, 'ColorOrderIndex', 2);
+plot(f, p_id_geo2, 'HandleVisibility',  'off');
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
+title('Huddle Test - Geophones')
+legend('location', 'northeast');
+xlim([1e-1, 5e3]); ylim([1e-11, 1e-4]);
+
+% Compute transfer functions
+% The transfer functions from the motion as measured by the interferometer (and that should represent the absolute motion of the mass) to the inertial sensors are estimated:
+
+[tf_acc1_est, f] = tfestimate(id.d, id.acc_1, win, [], [], 1/Ts);
+[co_acc1_est, ~] = mscohere(  id.d, id.acc_1, win, [], [], 1/Ts);
+[tf_acc2_est, ~] = tfestimate(id.d, id.acc_2, win, [], [], 1/Ts);
+[co_acc2_est, ~] = mscohere(  id.d, id.acc_2, win, [], [], 1/Ts);
+
+[tf_geo1_est, ~] = tfestimate(id.d, id.geo_1, win, [], [], 1/Ts);
+[co_geo1_est, ~] = mscohere(  id.d, id.geo_1, win, [], [], 1/Ts);
+[tf_geo2_est, ~] = tfestimate(id.d, id.geo_2, win, [], [], 1/Ts);
+[co_geo2_est, ~] = mscohere(  id.d, id.geo_2, win, [], [], 1/Ts);
+
+
+
+% The obtained coherence are shown in Figure [[fig:id_sensor_dynamics_coherence]].
+
+
+figure;
+hold on;
+set(gca, 'ColorOrderIndex', 1);
+plot(f, co_acc1_est, '-', 'DisplayName', 'Accelerometer')
+set(gca, 'ColorOrderIndex', 1);
+plot(f, co_acc2_est, '-',  'HandleVisibility', 'off')
+set(gca, 'ColorOrderIndex', 2);
+plot(f, co_geo1_est, '-', 'DisplayName', 'Geophone')
+set(gca, 'ColorOrderIndex', 2);
+plot(f, co_geo2_est, '-',  'HandleVisibility', 'off')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Coherence'); xlabel('Frequency [Hz]');
+hold off;
+xlim([2, 2e3]); ylim([0, 1])
+legend();
+
+
+
+% #+name: fig:id_sensor_dynamics_coherence
+% #+caption: Coherence for the estimation of the sensor dynamics
+% #+RESULTS:
+% [[file:figs/id_sensor_dynamics_coherence.png]]
+
+% We also make a simplified model of the inertial sensors to be compared with the identified dynamics.
+
+G_acc = 1/(1 + s/2/pi/2500); % [V/(m/s2)]
+G_geo = -1200*s^2/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2); % [[V/(m/s)]
+
+
+
+% The model and identified dynamics show good agreement (Figures [[fig:id_sensor_dynamics_accelerometers]] and [[fig:id_sensor_dynamics_geophones]].)
+
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(tf_acc1_est./(1i*2*pi*f).^2), '.')
+plot(f, abs(tf_acc2_est./(1i*2*pi*f).^2), '.')
+plot(f, abs(squeeze(freqresp(G_acc, f, 'Hz'))), 'k-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude $\left[\frac{V}{m/s^2}\right]$');  set(gca, 'XTickLabel',[]);
+hold off;
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(tf_acc1_est./(1i*2*pi*f).^2), '.')
+plot(f, 180/pi*angle(tf_acc2_est./(1i*2*pi*f).^2), '.')
+plot(f, 180/pi*angle(squeeze(freqresp(G_acc, f, 'Hz'))), 'k-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2], 'x');
+xlim([2, 2e3]);
+
+
+
+% #+name: fig:id_sensor_dynamics_accelerometers
+% #+caption: Identified dynamics of the accelerometers
+% #+RESULTS:
+% [[file:figs/id_sensor_dynamics_accelerometers.png]]
+
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(tf_geo1_est./(1i*2*pi*f)), '.')
+plot(f, abs(tf_geo2_est./(1i*2*pi*f)), '.')
+plot(f, abs(squeeze(freqresp(G_geo, f, 'Hz'))), 'k-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude $\left[\frac{V}{m/s}\right]$');  set(gca, 'XTickLabel',[]);
+hold off;
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(tf_geo1_est./(1i*2*pi*f)), '.')
+plot(f, 180/pi*angle(tf_geo2_est./(1i*2*pi*f)), '.')
+plot(f, 180/pi*angle(squeeze(freqresp(G_geo, f, 'Hz'))), 'k-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2], 'x');
+xlim([0.5, 2e3]);

matlab/inertial_sensor_noise.m

@@ -0,0 +1,224 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+addpath('./mat/');
+
+% Load Data
+% As before, the identification data is loaded and any offset if removed.
+
+id = load('identification_noise_opt_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
+
+id.d = detrend(id.d, 0);
+id.acc_1 = detrend(id.acc_1, 0);
+id.acc_2 = detrend(id.acc_2, 0);
+id.geo_1 = detrend(id.geo_1, 0);
+id.geo_2 = detrend(id.geo_2, 0);
+id.f_meas = detrend(id.f_meas, 0);
+
+% ASD of the Measured displacement
+% The Power Spectral Density of the displacement as measured by the interferometer and the inertial sensors is computed.
+
+Ts = id.t(2) - id.t(1);
+win = hann(ceil(10/Ts));
+
+[p_id_d,     f] = pwelch(id.d,      win, [], [], 1/Ts);
+[p_id_acc1,  ~] = pwelch(id.acc_1,  win, [], [], 1/Ts);
+[p_id_acc2,  ~] = pwelch(id.acc_2,  win, [], [], 1/Ts);
+[p_id_geo1,  ~] = pwelch(id.geo_1,  win, [], [], 1/Ts);
+[p_id_geo2,  ~] = pwelch(id.geo_2,  win, [], [], 1/Ts);
+
+
+
+% Let's use a model of the accelerometer and geophone to compute the motion from the measured voltage.
+
+G_acc = 1/(1 + s/2/pi/2500); % [V/(m/s2)]
+G_geo = -1200*s^2/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2); % [[V/(m/s)]
+
+
+
+% The obtained ASD in $m/\sqrt{Hz}$ is shown in Figure [[fig:measure_displacement_all_sensors]].
+
+
+figure;
+hold on;
+set(gca, 'ColorOrderIndex', 1);
+plot(f, sqrt(p_id_acc1)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
+     'DisplayName', 'Accelerometer');
+set(gca, 'ColorOrderIndex', 1);
+plot(f, sqrt(p_id_acc2)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
+     'HandleVisibility', 'off');
+set(gca, 'ColorOrderIndex', 2);
+plot(f, sqrt(p_id_geo1)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
+     'DisplayName', 'Geophone');
+set(gca, 'ColorOrderIndex', 2);
+plot(f, sqrt(p_id_geo2)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
+     'HandleVisibility', 'off');
+set(gca, 'ColorOrderIndex', 3);
+plot(f, sqrt(p_id_d), 'DisplayName', 'Interferometer');
+hold off;
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('ASD [$m/\sqrt{Hz}$]'); xlabel('Frequency [Hz]');
+title('Huddle Test')
+legend();
+xlim([1e-1, 5e3]); ylim([1e-12, 1e-4]);
+
+% ASD of the Sensor Noise
+% The noise of a sensor can be estimated using two identical sensors by computing:
+% - the Power Spectral Density of the measured motion by the two sensors
+% - the Cross Spectral Density between the two sensors (coherence)
+
+% This technique to estimate the sensor noise is described in cite:barzilai98_techn_measur_noise_sensor_presen.
+
+% The Power Spectral Density of the sensor noise can be estimated using the following equation:
+% \begin{equation}
+%   |S_n(\omega)| = |S_{x_1}(\omega)| \Big( 1 - \gamma_{x_1 x_2}(\omega) \Big)
+% \end{equation}
+% with $S_{x_1}$ the PSD of one of the sensor and $\gamma_{x_1 x_2}$ the coherence between the two sensors.
+
+% The coherence between the two accelerometers and between the two geophones is computed.
+
+[coh_acc, ~] = mscohere(id.acc_1, id.acc_2, win, [], [], 1/Ts);
+[coh_geo, ~] = mscohere(id.geo_1, id.geo_2, win, [], [], 1/Ts);
+
+
+
+% Finally, the Power Spectral Density of the sensors is computed and converted in $[m^2/Hz]$.
+
+pN_acc = p_id_acc1.*(1 - coh_acc) .* ... % [V^2/Hz]
+         1./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))).^2; % [(m/V)^2]
+pN_geo = p_id_geo1.*(1 - coh_geo) .* ... % [V^2/Hz]
+         1./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))).^2; % [(m/V)^2]
+
+
+
+% The ASD of obtained noises are compared with the ASD of the measured signals in Figure [[fig:noise_inertial_sensors_comparison]].
+
+figure;
+hold on;
+plot(f, sqrt(p_id_acc1)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
+     'DisplayName', 'Accelerometer');
+plot(f, sqrt(p_id_geo1)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
+     'DisplayName', 'Geophone');
+plot(f, sqrt(pN_acc), '-', 'DisplayName', 'Accelerometers - Noise');
+plot(f, sqrt(pN_geo), '-', 'DisplayName', 'Geophones - Noise');
+hold off;
+set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
+xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m}{\sqrt{Hz}}\right]$');
+xlim([1, 5000]); ylim([1e-12, 1e-5]);
+legend('location', 'northeast');
+
+% Noise Model
+% Transfer functions are adjusted in order to fit the ASD of the sensor noises (expressed in $[m/s/\sqrt{Hz}]$ for more easy fitting).
+
+% These transfer functions are defined below and compared with the measured ASD in Figure [[fig:noise_models_velocity]].
+
+N_acc = 1*(s/(2*pi*2000) + 1)^2/(s + 0.1*2*pi)/(s + 1e3*2*pi); % [m/sqrt(Hz)]
+N_geo = 4e-4*(s/(2*pi*200) + 1)/(s + 1e3*2*pi); % [m/sqrt(Hz)]
+
+freqs = logspace(0, 4, 1000);
+
+figure;
+hold on;
+plot(f, sqrt(pN_acc).*(2*pi*f), '-', 'DisplayName', 'Accelerometers - Noise');
+plot(f, sqrt(pN_geo).*(2*pi*f), '-', 'DisplayName', 'Geophones - Noise');
+set(gca, 'ColorOrderIndex', 1);
+plot(freqs, abs(squeeze(freqresp(N_acc, freqs, 'Hz'))), '--', 'DisplayName', 'Accelerometer - Noise Model');
+plot(freqs, abs(squeeze(freqresp(N_geo, freqs, 'Hz'))), '--', 'DisplayName', 'Geophones - Noise Model');
+hold off;
+set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
+xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m/s}{\sqrt{Hz}}\right]$');
+xlim([1, 5000]);
+legend('location', 'northeast');
+
+% $\mathcal{H}_2$ Synthesis of the Complementary Filters
+% We now wish to synthesize two complementary filters to merge the geophone and the accelerometer signal in such a way that the fused signal has the lowest possible RMS noise.
+
+% To do so, we use the $\mathcal{H}_2$ synthesis where the transfer functions representing the noise density of both sensors are used as weights.
+
+% The generalized plant used for the synthesis is defined below.
+
+P = [0      N_acc  1;
+     N_geo -N_acc  0];
+
+
+
+% And the $\mathcal{H}_2$ synthesis is done using the =h2syn= command.
+
+[H_geo, ~, gamma] = h2syn(P, 1, 1);
+H_acc = 1 - H_geo;
+
+
+
+% The obtained complementary filters are shown in Figure [[fig:complementary_filters_velocity_H2]].
+
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-', 'DisplayName', '$H_{acc}$');
+plot(freqs, abs(squeeze(freqresp(H_geo, freqs, 'Hz'))), '-', 'DisplayName', '$H_{geo}$');
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Magnitude');  set(gca, 'XTickLabel',[]);
+hold off;
+legend('location', 'northeast');
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-');
+plot(freqs, 180/pi*angle(squeeze(freqresp(H_geo, freqs, 'Hz'))), '-');
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2], 'x');
+xlim([freqs(1), freqs(end)]);
+
+% Results
+% Finally, the signals of both sensors are merged using the complementary filters and the super sensor noise is estimated and compared with the individual sensor noises in Figure [[fig:super_sensor_noise_asd_velocity]].
+
+
+freqs = logspace(0, 4, 1000);
+
+figure;
+hold on;
+plot(f, pN_acc.*(2*pi*f), '-', 'DisplayName', 'Accelerometers - Noise');
+plot(f, pN_geo.*(2*pi*f), '-', 'DisplayName', 'Geophones - Noise');
+plot(f, sqrt((pN_acc.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_acc, f, 'Hz'))).^2 + (pN_geo.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_geo, f, 'Hz'))).^2), 'k-', 'DisplayName', 'Super Sensor - Noise');
+hold off;
+set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
+xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m/s}{\sqrt{Hz}}\right]$');
+xlim([1, 5000]);
+legend('location', 'northeast');
+
+
+
+% #+name: fig:super_sensor_noise_asd_velocity
+% #+caption: ASD of the super sensor noise (velocity)
+% #+RESULTS:
+% [[file:figs/super_sensor_noise_asd_velocity.png]]
+
+% Finally, the Cumulative Power Spectrum is computed and compared in Figure [[fig:super_sensor_noise_cas_velocity]].
+
+[~, i_1Hz] = min(abs(f - 1));
+
+CPS_acc = 1/pi*flip(-cumtrapz(2*pi*flip(f), flip((pN_acc.*(2*pi*f)).^2)));
+CPS_geo = 1/pi*flip(-cumtrapz(2*pi*flip(f), flip((pN_geo.*(2*pi*f)).^2)));
+CPS_SS  = 1/pi*flip(-cumtrapz(2*pi*flip(f), flip((pN_acc.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_acc, f, 'Hz'))).^2 + (pN_geo.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_geo, f, 'Hz'))).^2)));
+
+figure;
+hold on;
+plot(f, CPS_acc, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_{acc}} = %.0f\\,\\mu m/s (rms)$', 1e6*sqrt(CPS_acc(i_1Hz))));
+plot(f, CPS_geo, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_{geo}} = %.0f\\,\\mu m/s (rms)$', 1e6*sqrt(CPS_geo(i_1Hz))));
+plot(f, CPS_SS,  'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}} = %.0f\\,\\mu m/s (rms)$',       1e6*sqrt(CPS_SS(i_1Hz))));
+set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
+hold off;
+xlim([1, 4e3]);
+legend('location', 'northeast');

matlab/inertial_sensor_uncertainty.m

@@ -0,0 +1,326 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+addpath('./mat/');
+addpath('./src/');
+
+% Load Data
+% Data is loaded and offset is removed.
+
+
+id = load('identification_noise_opt_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
+
+id.d = detrend(id.d, 0);
+id.acc_1 = detrend(id.acc_1, 0);
+id.acc_2 = detrend(id.acc_2, 0);
+id.geo_1 = detrend(id.geo_1, 0);
+id.geo_2 = detrend(id.geo_2, 0);
+id.f_meas = detrend(id.f_meas, 0);
+
+% Compute the dynamics of both sensors
+% The dynamics of inertial sensors are estimated (in $[V/m]$).
+
+Ts = id.t(2) - id.t(1);
+win = hann(ceil(10/Ts));
+
+[tf_acc1_est, f] = tfestimate(id.d, id.acc_1, win, [], [], 1/Ts);
+[co_acc1_est, ~] = mscohere(  id.d, id.acc_1, win, [], [], 1/Ts);
+[tf_acc2_est, ~] = tfestimate(id.d, id.acc_2, win, [], [], 1/Ts);
+[co_acc2_est, ~] = mscohere(  id.d, id.acc_2, win, [], [], 1/Ts);
+
+[tf_geo1_est, ~] = tfestimate(id.d, id.geo_1, win, [], [], 1/Ts);
+[co_geo1_est, ~] = mscohere(  id.d, id.geo_1, win, [], [], 1/Ts);
+[tf_geo2_est, ~] = tfestimate(id.d, id.geo_2, win, [], [], 1/Ts);
+[co_geo2_est, ~] = mscohere(  id.d, id.geo_2, win, [], [], 1/Ts);
+
+
+
+% The (nominal) models of the inertial sensors from the absolute displacement to the generated voltage are defined below:
+
+G_acc = 1/(1 + s/2/pi/2000)
+G_geo = -1200*s^2/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2);
+
+% Dynamics uncertainty estimation
+% Weights representing the dynamical uncertainty of the sensors are defined below.
+
+w_acc = createWeight('n', 2, 'G0', 10,  'G1', 0.2,    'Gc', 1,     'w0', 6*2*pi) * ...
+        createWeight('n', 2, 'G0', 1,   'G1', 5/0.2,  'Gc', 1/0.2, 'w0', 1300*2*pi);
+
+w_geo = createWeight('n', 2, 'G0', 0.6, 'G1', 0.2,    'Gc', 0.3,   'w0', 3*2*pi) * ...
+        createWeight('n', 2, 'G0', 1,   'G1', 10/0.2, 'Gc', 1/0.2, 'w0', 800*2*pi);
+
+
+
+% The measured dynamics are compared with the modelled one as well as the modelled uncertainty in Figure [[fig:dyn_uncertainty_acc]] for the accelerometers and in Figure [[fig:dyn_uncertainty_geo]] for the geophones.
+
+
+figure;
+tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+% Magnitude
+ax1 = nexttile;
+hold on;
+plotMagUncertainty(w_acc, freqs, 'G', G_acc, 'color_i', 1, 'DisplayName', '$G_{acc}$');
+set(gca,'ColorOrderIndex',1)
+plot(f, abs(tf_acc1_est./(1i*2*pi*f).^2), '.', 'DisplayName', 'Meaurement')
+set(gca,'ColorOrderIndex',1)
+plot(f, abs(tf_acc2_est./(1i*2*pi*f).^2), '.', 'HandleVisibility', 'off')
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(G_acc, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_{acc}$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Magnitude $[\frac{V}{m}]$');
+legend('location', 'southwest', 'FontSize', 8);
+hold off;
+ylim([1e-3, 1e1])
+
+% Phase
+ax2 = nexttile;
+hold on;
+plotPhaseUncertainty(w_acc, freqs, 'G', G_acc, 'color_i', 1);
+set(gca,'ColorOrderIndex',1)
+plot(f, 180/pi*angle(tf_acc1_est./(1i*2*pi*f).^2), '.');
+set(gca,'ColorOrderIndex',1)
+plot(f, 180/pi*angle(tf_acc2_est./(1i*2*pi*f).^2), '.');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_acc, freqs, 'Hz'))));
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 5e3]);
+
+
+
+% #+name: fig:dyn_uncertainty_acc
+% #+caption: Modeled dynamical uncertainty and meaured dynamics of the accelerometers
+% #+RESULTS:
+% [[file:figs/dyn_uncertainty_acc.png]]
+
+
+figure;
+tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+% Magnitude
+ax1 = nexttile;
+hold on;
+plotMagUncertainty(w_geo, freqs, 'G', G_geo, 'color_i', 2, 'DisplayName', '$G_{geo}$');
+set(gca,'ColorOrderIndex',2)
+plot(f, abs(tf_geo1_est./(1i*2*pi*f)), '.', 'DisplayName', 'Measurement')
+set(gca,'ColorOrderIndex',2)
+plot(f, abs(tf_geo2_est./(1i*2*pi*f)), '.', 'HandleVisibility', 'off')
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(G_geo, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_{geo}$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Magnitude $[\frac{V}{m}]$');
+legend('location', 'northwest', 'FontSize', 8);
+hold off;
+ylim([1, 1e4])
+
+% Phase
+ax2 = nexttile;
+hold on;
+plotPhaseUncertainty(w_geo, freqs, 'G', G_geo, 'color_i', 2);
+set(gca,'ColorOrderIndex',2)
+plot(f, 180/pi*unwrap(angle(tf_geo1_est./(1i*2*pi*f)))+360, '.');
+set(gca,'ColorOrderIndex',2)
+plot(f, 180/pi*unwrap(angle(tf_geo2_est./(1i*2*pi*f))), '.');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_geo, freqs, 'Hz'))));
+set(gca,'xscale','log');
+yticks(-270:90:180);
+ylim([-270 90]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 5e3]);
+
+% $\mathcal{H}_\infty$ Synthesis of Complementary Filters
+% A last weight is now defined that represents the maximum dynamical uncertainty that is allowed for the super sensor.
+
+wu = inv(createWeight('n', 2, 'G0', 0.7, 'G1', 0.3,   'Gc', 0.4,   'w0', 3*2*pi) * ...
+         createWeight('n', 2, 'G0', 1,   'G1', 6/0.3, 'Gc', 1/0.3, 'w0', 1200*2*pi));
+
+
+
+% This dynamical uncertainty is compared with the two sensor uncertainties in Figure [[fig:uncertainty_weight_and_sensor_uncertainties]].
+
+Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))));
+Dphi_Wu(abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))) > 1) = 360;
+
+figure;
+tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+% Magnitude
+ax1 = nexttile;
+hold on;
+plotMagUncertainty(w_acc, freqs, 'color_i', 1, 'DisplayName', '$1 + W_{acc} \Delta$');
+plotMagUncertainty(w_geo, freqs, 'color_i', 2, 'DisplayName', '$1 + W_{geo} \Delta$');
+plot(freqs, 1 + abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))), 'k--', ...
+     'DisplayName', '$1 + W_u^{-1} \Delta$')
+plot(freqs, 1 - abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))), 'k--', ...
+     'HandleVisibility', 'off')
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Magnitude');
+ylim([1e-2, 1e1]);
+legend('location', 'southeast', 'FontSize', 8);
+hold off;
+
+% Phase
+ax2 = nexttile;
+hold on;
+plotPhaseUncertainty(w_acc, freqs, 'color_i', 1);
+plotPhaseUncertainty(w_geo, freqs, 'color_i', 2);
+plot(freqs,  Dphi_Wu, 'k--');
+plot(freqs, -Dphi_Wu, 'k--');
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+
+
+% #+name: fig:uncertainty_weight_and_sensor_uncertainties
+% #+caption: Individual sensor uncertainty (normalized by their dynamics) and the wanted maximum super sensor noise uncertainty
+% #+RESULTS:
+% [[file:figs/uncertainty_weight_and_sensor_uncertainties.png]]
+
+% The generalized plant used for the synthesis is defined:
+
+P = [wu*w_acc -wu*w_acc;
+     0         wu*w_geo;
+     1         0];
+
+
+
+% And the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command is performed.
+
+[H_geo, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
+
+
+
+% #+RESULTS:
+% #+begin_example
+%   Test bounds:  0.8556 <=  gamma  <=  1.34
+
+%     gamma        X>=0        Y>=0       rho(XY)<1    p/f
+%   1.071e+00     0.0e+00     0.0e+00     0.000e+00     p
+%   9.571e-01     0.0e+00     0.0e+00     9.436e-16     p
+%   9.049e-01     0.0e+00     0.0e+00     1.084e-15     p
+%   8.799e-01     0.0e+00     0.0e+00     1.191e-16     p
+%   8.677e-01     0.0e+00     0.0e+00     6.905e-15     p
+%   8.616e-01     0.0e+00     0.0e+00     0.000e+00     p
+%   8.586e-01     1.1e-17     0.0e+00     6.917e-16     p
+%   8.571e-01     0.0e+00     0.0e+00     6.991e-17     p
+%   8.564e-01     0.0e+00     0.0e+00     1.492e-16     p
+
+%   Best performance (actual): 0.8563
+% #+end_example
+
+% The complementary filter is defined as follows:
+
+H_acc = 1 - H_geo;
+
+
+
+% The bode plot of the obtained complementary filters is shown in Figure
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+% Magnitude
+ax1 = nexttile([2,1]);
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 1./abs(squeeze(freqresp(w_geo, freqs, 'Hz'))), '--', 'DisplayName', '$w_{geo}$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 1./abs(squeeze(freqresp(w_acc, freqs, 'Hz'))), '--', 'DisplayName', '$w_{acc}$');
+
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(H_geo, freqs, 'Hz'))), '-', 'DisplayName', '$H_{geo}$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-', 'DisplayName', '$H_{acc}$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Magnitude');
+set(gca, 'XTickLabel',[]);
+ylim([1e-2, 1e1]);
+legend('location', 'southeast');
+
+ax2 = nexttile;
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*phase(squeeze(freqresp(H_geo, freqs, 'Hz'))), '-');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 180/pi*phase(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-');
+hold off;
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+set(gca, 'XScale', 'log');
+yticks([-360:90:360]);
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+
+% Obtained Super Sensor Dynamical Uncertainty
+% The obtained super sensor dynamical uncertainty is shown in Figure [[fig:super_sensor_uncertainty_h_infinity]].
+
+
+Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))));
+Dphi_Wu(abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))) > 1) = 360;
+
+Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz'))) + abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))));
+Dphi_ss(abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz'))) + abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))) > 1) = 360;
+
+figure;
+tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+% Magnitude
+ax1 = nexttile;
+hold on;
+plotMagUncertainty(w_acc, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
+plotMagUncertainty(w_geo, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
+plot(freqs, 1 + abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz')))+abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))), 'k-', ...
+     'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$')
+plot(freqs, max(1 - abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz')))-abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))), 0.001), 'k-', ...
+     'HandleVisibility', 'off');
+plot(freqs, 1 + abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))), 'k--', ...
+     'DisplayName', '$1 + W_u^{-1}\Delta$')
+plot(freqs, 1 - abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))), 'k--', ...
+     'HandleVisibility', 'off')
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Magnitude');
+ylim([1e-2, 1e1]);
+legend('location', 'southeast', 'FontSize', 8);
+hold off;
+
+% Phase
+ax2 = nexttile;
+hold on;
+plotPhaseUncertainty(w_acc, freqs, 'color_i', 1);
+plotPhaseUncertainty(w_geo, freqs, 'color_i', 2);
+plot(freqs,  Dphi_ss, 'k-');
+plot(freqs, -Dphi_ss, 'k-');
+plot(freqs,  Dphi_Wu, 'k--');
+plot(freqs, -Dphi_Wu, 'k--');
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);

matlab/integral_force_feedback.m

@@ -0,0 +1,226 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+addpath('./mat/');
+
+% Load Data
+% The experimental data is loaded and any offset is removed.
+
+id_ol = load('identification_noise_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
+
+id_ol.d = detrend(id_ol.d, 0);
+id_ol.acc_1 = detrend(id_ol.acc_1, 0);
+id_ol.acc_2 = detrend(id_ol.acc_2, 0);
+id_ol.geo_1 = detrend(id_ol.geo_1, 0);
+id_ol.geo_2 = detrend(id_ol.geo_2, 0);
+id_ol.f_meas = detrend(id_ol.f_meas, 0);
+id_ol.u = detrend(id_ol.u, 0);
+
+% Experimental Data
+% The transfer function from force actuator to force sensors is estimated.
+
+% The coherence shown in Figure [[fig:iff_identification_coh]] shows that the excitation signal is good enough.
+
+
+Ts = id_ol.t(2) - id_ol.t(1);
+win = hann(ceil(10/Ts));
+
+[tf_fmeas_est, f] = tfestimate(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts); % [V/m]
+[co_fmeas_est, ~] = mscohere(  id_ol.u, id_ol.f_meas, win, [], [], 1/Ts);
+
+figure;
+hold on;
+plot(f, co_fmeas_est, '-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Coherence'); xlabel('Frequency [Hz]');
+hold off;
+xlim([1, 1e3]); ylim([0, 1])
+
+
+
+% #+name: fig:iff_identification_coh
+% #+caption: Coherence for the identification of the IFF plant
+% #+RESULTS:
+% [[file:figs/iff_identification_coh.png]]
+
+% The obtained dynamics is shown in Figure [[fig:iff_identification_bode_plot]].
+
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(tf_fmeas_est), '-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
+hold off;
+ylim([1e-1, 1e3]);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(tf_fmeas_est), '-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2], 'x');
+xlim([1, 1e3]);
+
+% Model of the IFF Plant
+% In order to plot the root locus for the IFF control strategy, a model of the identified plant is developed.
+
+% It consists of several poles and zeros are shown below.
+
+wz = 2*pi*102;
+xi_z = 0.01;
+wp = 2*pi*239.4;
+xi_p = 0.015;
+
+Giff = 2.2*(s^2 + 2*xi_z*s*wz + wz^2)/(s^2 + 2*xi_p*s*wp + wp^2) * ... % Dynamics
+       10*(s/3/pi/(1 + s/3/pi)) * ... % Low pass filter and gain of the voltage amplifier
+       exp(-Ts*s); % Time delay induced by ADC/DAC
+
+
+
+% The comparison of the identified dynamics and the developed model is done in Figure [[fig:iff_plant_model]].
+
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(tf_fmeas_est), '.')
+plot(f, abs(squeeze(freqresp(Giff, f, 'Hz'))), 'k-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
+hold off;
+ylim([1e-2, 1e3])
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(tf_fmeas_est), '.')
+plot(f, 180/pi*angle(squeeze(freqresp(Giff, f, 'Hz'))), 'k-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2], 'x');
+xlim([0.5, 5e3]);
+
+% Root Locus and optimal Controller
+% Now, the root locus for the Integral Force Feedback strategy is computed and shown in Figure [[fig:iff_root_locus]].
+
+% Note that the controller used is not a pure integrator but rather a first order low pass filter with a cut-off frequency set at 2Hz.
+
+
+gains = logspace(0, 5, 1000);
+
+figure;
+hold on;
+plot(real(pole(Giff)),  imag(pole(Giff)),  'kx');
+plot(real(tzero(Giff)),  imag(tzero(Giff)),  'ko');
+for i = 1:length(gains)
+    cl_poles = pole(feedback(Giff, gains(i)/(s + 2*pi*2)));
+    plot(real(cl_poles), imag(cl_poles), 'k.');
+end
+cl_poles = pole(feedback(Giff, 102/(s + 2*pi*2)));
+plot(real(cl_poles), imag(cl_poles), 'rx');
+ylim([0, 1800]);
+xlim([-1600,200]);
+xlabel('Real Part')
+ylabel('Imaginary Part')
+axis square
+
+
+
+% #+name: fig:iff_root_locus
+% #+caption: Root Locus for the IFF control
+% #+RESULTS:
+% [[file:figs/iff_root_locus.png]]
+
+% The controller that yield maximum damping (shown by the red cross in Figure [[fig:iff_root_locus]]) is:
+
+Kiff_opt = 102/(s + 2*pi*2);
+
+% Verification of the achievable damping
+% A new identification is performed with the IFF control strategy applied to the system.
+
+% Data is loaded and offset removed.
+
+id_cl = load('identification_noise_iff_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
+
+id_cl.d      = detrend(id_cl.d, 0);
+id_cl.acc_1  = detrend(id_cl.acc_1, 0);
+id_cl.acc_2  = detrend(id_cl.acc_2, 0);
+id_cl.geo_1  = detrend(id_cl.geo_1, 0);
+id_cl.geo_2  = detrend(id_cl.geo_2, 0);
+id_cl.f_meas = detrend(id_cl.f_meas, 0);
+id_cl.u      = detrend(id_cl.u, 0);
+
+
+
+% The open-loop and closed-loop dynamics are estimated.
+
+[tf_G_ol_est, f] = tfestimate(id_ol.u, id_ol.d, win, [], [], 1/Ts);
+[co_G_ol_est, ~] = mscohere(  id_ol.u, id_ol.d, win, [], [], 1/Ts);
+[tf_G_cl_est, ~] = tfestimate(id_cl.u, id_cl.d, win, [], [], 1/Ts);
+[co_G_cl_est, ~] = mscohere(  id_cl.u, id_cl.d, win, [], [], 1/Ts);
+
+
+
+% The obtained coherence is shown in Figure [[fig:Gd_identification_iff_coherence]] and the dynamics in Figure [[fig:Gd_identification_iff_bode_plot]].
+
+
+figure;
+hold on;
+plot(f, co_G_ol_est, '-', 'DisplayName', 'OL')
+plot(f, co_G_cl_est, '-', 'DisplayName', 'IFF')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Coherence'); xlabel('Frequency [Hz]');
+hold off;
+xlim([1, 1e3]); ylim([0, 1])
+legend('location', 'southwest');
+
+
+
+% #+name: fig:Gd_identification_iff_coherence
+% #+caption: Coherence for the transfer function from F to d, with and without IFF
+% #+RESULTS:
+% [[file:figs/Gd_identification_iff_coherence.png]]
+
+
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(tf_G_ol_est), '-', 'DisplayName', 'OL')
+plot(f, abs(tf_G_cl_est), '-', 'DisplayName', 'IFF')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+hold off;
+legend('location', 'northeast');
+ylim([2e-7, 2e-4]);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(tf_G_ol_est), '-')
+plot(f, 180/pi*angle(tf_G_cl_est), '-')
+set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
+ylabel('Phase'); xlabel('Frequency [Hz]');
+hold off;
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+
+linkaxes([ax1,ax2], 'x');
+xlim([1, 1e3]);