Simlink index.html

.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/htmlize.css

@@ -1,145 +0,0 @@
-.org-bold { /* bold */ font-weight: bold; }
-.org-bold-italic { /* bold-italic */ font-weight: bold; font-style: italic; }
-.org-buffer-menu-buffer { /* buffer-menu-buffer */ font-weight: bold; }
-.org-builtin { /* font-lock-builtin-face */ color: #7a378b; }
-.org-button { /* button */ text-decoration: underline; }
-.org-calendar-today { /* calendar-today */ text-decoration: underline; }
-.org-change-log-acknowledgement { /* change-log-acknowledgement */ color: #b22222; }
-.org-change-log-conditionals { /* change-log-conditionals */ color: #a0522d; }
-.org-change-log-date { /* change-log-date */ color: #8b2252; }
-.org-change-log-email { /* change-log-email */ color: #a0522d; }
-.org-change-log-file { /* change-log-file */ color: #0000ff; }
-.org-change-log-function { /* change-log-function */ color: #a0522d; }
-.org-change-log-list { /* change-log-list */ color: #a020f0; }
-.org-change-log-name { /* change-log-name */ color: #008b8b; }
-.org-comint-highlight-input { /* comint-highlight-input */ font-weight: bold; }
-.org-comint-highlight-prompt { /* comint-highlight-prompt */ color: #00008b; }
-.org-comment { /* font-lock-comment-face */ color: #999988; font-style: italic; }
-.org-comment-delimiter { /* font-lock-comment-delimiter-face */ color: #999988; font-style: italic; }
-.org-completions-annotations { /* completions-annotations */ font-style: italic; }
-.org-completions-common-part { /* completions-common-part */ color: #000000; background-color: #ffffff; }
-.org-completions-first-difference { /* completions-first-difference */ font-weight: bold; }
-.org-constant { /* font-lock-constant-face */ color: #008b8b; }
-.org-diary { /* diary */ color: #ff0000; }
-.org-diff-context { /* diff-context */ color: #7f7f7f; }
-.org-diff-file-header { /* diff-file-header */ background-color: #b3b3b3; font-weight: bold; }
-.org-diff-function { /* diff-function */ background-color: #cccccc; }
-.org-diff-header { /* diff-header */ background-color: #cccccc; }
-.org-diff-hunk-header { /* diff-hunk-header */ background-color: #cccccc; }
-.org-diff-index { /* diff-index */ background-color: #b3b3b3; font-weight: bold; }
-.org-diff-nonexistent { /* diff-nonexistent */ background-color: #b3b3b3; font-weight: bold; }
-.org-diff-refine-change { /* diff-refine-change */ background-color: #d9d9d9; }
-.org-dired-directory { /* dired-directory */ color: #0000ff; }
-.org-dired-flagged { /* dired-flagged */ color: #ff0000; font-weight: bold; }
-.org-dired-header { /* dired-header */ color: #228b22; }
-.org-dired-ignored { /* dired-ignored */ color: #7f7f7f; }
-.org-dired-mark { /* dired-mark */ color: #008b8b; }
-.org-dired-marked { /* dired-marked */ color: #ff0000; font-weight: bold; }
-.org-dired-perm-write { /* dired-perm-write */ color: #b22222; }
-.org-dired-symlink { /* dired-symlink */ color: #a020f0; }
-.org-dired-warning { /* dired-warning */ color: #ff0000; font-weight: bold; }
-.org-doc { /* font-lock-doc-face */ color: #8b2252; }
-.org-escape-glyph { /* escape-glyph */ color: #a52a2a; }
-.org-file-name-shadow { /* file-name-shadow */ color: #7f7f7f; }
-.org-flyspell-duplicate { /* flyspell-duplicate */ color: #cdad00; font-weight: bold; text-decoration: underline; }
-.org-flyspell-incorrect { /* flyspell-incorrect */ color: #ff4500; font-weight: bold; text-decoration: underline; }
-.org-fringe { /* fringe */ background-color: #f2f2f2; }
-.org-function-name { /* font-lock-function-name-face */ color: teal; }
-.org-header-line { /* header-line */ color: #333333; background-color: #e5e5e5; }
-.org-help-argument-name { /* help-argument-name */ font-style: italic; }
-.org-highlight { /* highlight */ background-color: #b4eeb4; }
-.org-holiday { /* holiday */ background-color: #ffc0cb; }
-.org-isearch { /* isearch */ color: #b0e2ff; background-color: #cd00cd; }
-.org-isearch-fail { /* isearch-fail */ background-color: #ffc1c1; }
-.org-italic { /* italic */ font-style: italic; }
-.org-keyword { /* font-lock-keyword-face */ color: #0086b3; }
-.org-lazy-highlight { /* lazy-highlight */ background-color: #afeeee; }
-.org-link { /* link */ color: #0000ff; text-decoration: underline; }
-.org-link-visited { /* link-visited */ color: #8b008b; text-decoration: underline; }
-.org-log-edit-header { /* log-edit-header */ color: #a020f0; }
-.org-log-edit-summary { /* log-edit-summary */ color: #0000ff; }
-.org-log-edit-unknown-header { /* log-edit-unknown-header */ color: #b22222; }
-.org-match { /* match */ background-color: #ffff00; }
-.org-next-error { /* next-error */ background-color: #eedc82; }
-.org-nobreak-space { /* nobreak-space */ color: #a52a2a; text-decoration: underline; }
-.org-org-archived { /* org-archived */ color: #7f7f7f; }
-.org-org-block { /* org-block */ color: #7f7f7f; }
-.org-org-block-begin-line { /* org-block-begin-line */ color: #b22222; }
-.org-org-block-end-line { /* org-block-end-line */ color: #b22222; }
-.org-org-checkbox { /* org-checkbox */ font-weight: bold; }
-.org-org-checkbox-statistics-done { /* org-checkbox-statistics-done */ color: #228b22; font-weight: bold; }
-.org-org-checkbox-statistics-todo { /* org-checkbox-statistics-todo */ color: #ff0000; font-weight: bold; }
-.org-org-clock-overlay { /* org-clock-overlay */ background-color: #ffff00; }
-.org-org-code { /* org-code */ color: #7f7f7f; }
-.org-org-column { /* org-column */ background-color: #e5e5e5; }
-.org-org-column-title { /* org-column-title */ background-color: #e5e5e5; font-weight: bold; text-decoration: underline; }
-.org-org-date { /* org-date */ color: #a020f0; text-decoration: underline; }
-.org-org-document-info { /* org-document-info */ color: #191970; }
-.org-org-document-info-keyword { /* org-document-info-keyword */ color: #7f7f7f; }
-.org-org-document-title { /* org-document-title */ color: #191970; font-size: 144%; font-weight: bold; }
-.org-org-done { /* org-done */ color: #228b22; font-weight: bold; }
-.org-org-drawer { /* org-drawer */ color: #0000ff; }
-.org-org-ellipsis { /* org-ellipsis */ color: #b8860b; text-decoration: underline; }
-.org-org-footnote { /* org-footnote */ color: #a020f0; text-decoration: underline; }
-.org-org-formula { /* org-formula */ color: #b22222; }
-.org-org-headline-done { /* org-headline-done */ color: #bc8f8f; }
-.org-org-hide { /* org-hide */ color: #ffffff; }
-.org-org-latex-and-export-specials { /* org-latex-and-export-specials */ color: #8b4513; }
-.org-org-level-1 { /* org-level-1 */ color: #0000ff; }
-.org-org-level-2 { /* org-level-2 */ color: #a0522d; }
-.org-org-level-3 { /* org-level-3 */ color: #a020f0; }
-.org-org-level-4 { /* org-level-4 */ color: #b22222; }
-.org-org-level-5 { /* org-level-5 */ color: #228b22; }
-.org-org-level-6 { /* org-level-6 */ color: #008b8b; }
-.org-org-level-7 { /* org-level-7 */ color: #7a378b; }
-.org-org-level-8 { /* org-level-8 */ color: #8b2252; }
-.org-org-link { /* org-link */ color: #0000ff; text-decoration: underline; }
-.org-org-meta-line { /* org-meta-line */ color: #b22222; }
-.org-org-mode-line-clock { /* org-mode-line-clock */ color: #000000; background-color: #bfbfbf; }
-.org-org-mode-line-clock-overrun { /* org-mode-line-clock-overrun */ color: #000000; background-color: #ff0000; }
-.org-org-quote { /* org-quote */ color: #7f7f7f; }
-.org-org-scheduled { /* org-scheduled */ color: #006400; }
-.org-org-scheduled-previously { /* org-scheduled-previously */ color: #b22222; }
-.org-org-scheduled-today { /* org-scheduled-today */ color: #006400; }
-.org-org-sexp-date { /* org-sexp-date */ color: #a020f0; }
-.org-org-special-keyword { /* org-special-keyword */ color: #a020f0; }
-.org-org-table { /* org-table */ color: #0000ff; }
-.org-org-tag { /* org-tag */ font-weight: bold; }
-.org-org-target { /* org-target */ text-decoration: underline; }
-.org-org-time-grid { /* org-time-grid */ color: #b8860b; }
-.org-org-todo { /* org-todo */ color: #ff0000; font-weight: bold; }
-.org-org-upcoming-deadline { /* org-upcoming-deadline */ color: #b22222; }
-.org-org-verbatim { /* org-verbatim */ color: #7f7f7f; }
-.org-org-verse { /* org-verse */ color: #7f7f7f; }
-.org-org-warning { /* org-warning */ color: #ff0000; font-weight: bold; }
-.org-outline-1 { /* outline-1 */ color: #0000ff; }
-.org-outline-2 { /* outline-2 */ color: #a0522d; }
-.org-outline-3 { /* outline-3 */ color: #a020f0; }
-.org-outline-4 { /* outline-4 */ color: #b22222; }
-.org-outline-5 { /* outline-5 */ color: #228b22; }
-.org-outline-6 { /* outline-6 */ color: #008b8b; }
-.org-outline-7 { /* outline-7 */ color: #7a378b; }
-.org-outline-8 { /* outline-8 */ color: #8b2252; }
-.org-preprocessor { /* font-lock-preprocessor-face */ color: #7a378b; }
-.org-query-replace { /* query-replace */ color: #b0e2ff; background-color: #cd00cd; }
-.org-regexp-grouping-backslash { /* font-lock-regexp-grouping-backslash */ font-weight: bold; }
-.org-regexp-grouping-construct { /* font-lock-regexp-grouping-construct */ font-weight: bold; }
-.org-region { /* region */ background-color: #eedc82; }
-.org-secondary-selection { /* secondary-selection */ background-color: #ffff00; }
-.org-shadow { /* shadow */ color: #7f7f7f; }
-.org-show-paren-match { /* show-paren-match */ background-color: #40e0d0; }
-.org-show-paren-mismatch { /* show-paren-mismatch */ color: #ffffff; background-color: #a020f0; }
-.org-string { /* font-lock-string-face */ color: #dd1144; }
-.org-tool-bar { /* tool-bar */ color: #000000; background-color: #bfbfbf; }
-.org-tooltip { /* tooltip */ color: #000000; background-color: #ffffe0; }
-.org-trailing-whitespace { /* trailing-whitespace */ background-color: #ff0000; }
-.org-type { /* font-lock-type-face */ color: #228b22; }
-.org-underline { /* underline */ text-decoration: underline; }
-.org-variable-name { /* font-lock-variable-name-face */ color: teal; }
-.org-warning { /* font-lock-warning-face */ color: #ff0000; font-weight: bold; }
-.org-widget-button { /* widget-button */ font-weight: bold; }
-.org-widget-button-pressed { /* widget-button-pressed */ color: #ff0000; }
-.org-widget-documentation { /* widget-documentation */ color: #006400; }
-.org-widget-field { /* widget-field */ background-color: #d9d9d9; }
-.org-widget-inactive { /* widget-inactive */ color: #7f7f7f; }
-.org-widget-single-line-field { /* widget-single-line-field */ background-color: #d9d9d9; }

index.html

@@ -0,0 +1 @@
+test-bench-sensor-fusion.html

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

@@ -1,85 +0,0 @@
-$(function() {
-    $('.note').before("<p class='admonition-title note'>Note</p>");
-    $('.seealso').before("<p class='admonition-title seealso'>See also</p>");
-    $('.warning').before("<p class='admonition-title warning'>Warning</p>");
-    $('.caution').before("<p class='admonition-title caution'>Caution</p>");
-    $('.attention').before("<p class='admonition-title attention'>Attention</p>");
-    $('.tip').before("<p class='admonition-title tip'>Tip</p>");
-    $('.important').before("<p class='admonition-title important'>Important</p>");
-    $('.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>");
-});
-
-$( document ).ready(function() {
-
-    // Shift nav in mobile when clicking the menu.
-    $(document).on('click', "[data-toggle='wy-nav-top']", function() {
-      $("[data-toggle='wy-nav-shift']").toggleClass("shift");
-      $("[data-toggle='rst-versions']").toggleClass("shift");
-    });
-    // Close menu when you click a link.
-    $(document).on('click', ".wy-menu-vertical .current ul li a", function() {
-      $("[data-toggle='wy-nav-shift']").removeClass("shift");
-      $("[data-toggle='rst-versions']").toggleClass("shift");
-    });
-    $(document).on('click', "[data-toggle='rst-current-version']", function() {
-      $("[data-toggle='rst-versions']").toggleClass("shift-up");
-    });
-    // Make tables responsive
-    $("table.docutils:not(.field-list)").wrap("<div class='wy-table-responsive'></div>");
-});
-
-$( document ).ready(function() {
-    $('#text-table-of-contents ul').first().addClass('nav');
-                                        // ScrollSpy also requires that we use
-                                        // a Bootstrap nav component.
-    $('body').scrollspy({target: '#text-table-of-contents'});
-
-    // add sticky table headers
-    $('table').stickyTableHeaders();
-
-    // set the height of tableOfContents
-    var $postamble = $('#postamble');
-    var $tableOfContents = $('#table-of-contents');
-    $tableOfContents.css({paddingBottom: $postamble.outerHeight()});
-
-    // add TOC button
-    var toggleSidebar = $('<div id="toggle-sidebar"><a href="#table-of-contents"><h2>Table of Contents</h2></a></div>');
-    $('#content').prepend(toggleSidebar);
-
-    // add close button when sidebar showed in mobile screen
-    var closeBtn = $('<a class="close-sidebar" href="#">Close</a>');
-    var tocTitle = $('#table-of-contents').find('h2');
-    tocTitle.append(closeBtn);
-});
-
-window.SphinxRtdTheme = (function (jquery) {
-    var stickyNav = (function () {
-        var navBar,
-            win,
-            stickyNavCssClass = 'stickynav',
-            applyStickNav = function () {
-                if (navBar.height() <= win.height()) {
-                    navBar.addClass(stickyNavCssClass);
-                } else {
-                    navBar.removeClass(stickyNavCssClass);
-                }
-            },
-            enable = function () {
-                applyStickNav();
-                win.on('resize', applyStickNav);
-            },
-            init = function () {
-                navBar = jquery('nav.wy-nav-side:first');
-                win    = jquery(window);
-            };
-        jquery(init);
-        return {
-            enable : enable
-        };
-    }());
-    return {
-        StickyNav : stickyNav
-    };
-}($));

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]);