#+TITLE: SVD Control
:DRAWER:
#+STARTUP: overview

#+LANGUAGE: en
#+EMAIL: dehaeze.thomas@gmail.com
#+AUTHOR: Dehaeze Thomas

#+HTML_LINK_HOME: ../index.html
#+HTML_LINK_UP: ../index.html

#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/custom.css"/>
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/bootstrap.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/readtheorg.js"></script>

#+HTML_MATHJAX: align: center tagside: right font: TeX

#+PROPERTY: header-args:matlab  :session *MATLAB*
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :exports both
#+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :output-dir figs
#+PROPERTY: header-args:matlab+ :tangle no
#+PROPERTY: header-args:matlab+ :mkdirp yes

#+PROPERTY: header-args:shell  :eval no-export

#+PROPERTY: header-args:latex  :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/tikz/org/}{config.tex}")
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
#+PROPERTY: header-args:latex+ :results file raw replace
#+PROPERTY: header-args:latex+ :buffer no
#+PROPERTY: header-args:latex+ :eval no-export
#+PROPERTY: header-args:latex+ :exports results
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :output-dir figs
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
:END:

* Gravimeter - Simscape Model
:PROPERTIES:
:header-args:matlab+: :tangle gravimeter/script.m
:END:
** Introduction

#+name: fig:gravimeter_model
#+caption: Model of the gravimeter
[[file:figs/gravimeter_model.png]]

** Matlab Init                                              :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
#+end_src

#+begin_src matlab
  addpath('gravimeter');
#+end_src

** Simscape Model - Parameters
#+begin_src matlab
  open('gravimeter.slx')
#+end_src

Parameters
#+begin_src matlab
  l  = 1.0; % Length of the mass [m]
  la = 0.5; % Position of Act. [m]

  h  = 3.4; % Height of the mass [m]
  ha = 1.7; % Position of Act. [m]

  m = 400; % Mass [kg]
  I = 115; % Inertia [kg m^2]

  k = 15e3; % Actuator Stiffness [N/m]
  c = 0.03; % Actuator Damping [N/(m/s)]

  deq = 0.2; % Length of the actuators [m]

  g = 0; % Gravity [m/s2]
#+end_src

** System Identification - Without Gravity
#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'gravimeter';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/F1'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/F2'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/F3'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Acc_side'], 1, 'openoutput'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Acc_side'], 2, 'openoutput'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Acc_top'], 1, 'openoutput'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Acc_top'], 2, 'openoutput'); io_i = io_i + 1;

  G = linearize(mdl, io);
  G.InputName  = {'F1', 'F2', 'F3'};
  G.OutputName = {'Ax1', 'Az1', 'Ax2', 'Az2'};
#+end_src

#+begin_src matlab :results output replace :exports results
  pole(G)
#+end_src

#+RESULTS:
#+begin_example
pole(G)
ans =
      -0.000473481142385795 +      21.7596190728632i
      -0.000473481142385795 -      21.7596190728632i
      -7.49842879459172e-05 +       8.6593576906982i
      -7.49842879459172e-05 -       8.6593576906982i
       -5.1538686792578e-06 +      2.27025295182756i
       -5.1538686792578e-06 -      2.27025295182756i
#+end_example

The plant as 6 states as expected (2 translations + 1 rotation)
#+begin_src matlab :results output replace
  size(G)
#+end_src

#+RESULTS:
: State-space model with 4 outputs, 3 inputs, and 6 states.

#+begin_src matlab :exports none
  freqs = logspace(-2, 2, 1000);

  figure;
  for in_i = 1:3
      for out_i = 1:4
          subplot(4, 3, 3*(out_i-1)+in_i);
          plot(freqs, abs(squeeze(freqresp(G(out_i,in_i), freqs, 'Hz'))), '-');
          set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
      end
  end
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/open_loop_tf.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:open_loop_tf
#+caption: Open Loop Transfer Function from 3 Actuators to 4 Accelerometers
#+RESULTS:
[[file:figs/open_loop_tf.png]]

** System Identification - With Gravity
#+begin_src matlab
  g = 9.80665; % Gravity [m/s2]
#+end_src

#+begin_src matlab
  Gg = linearize(mdl, io);
  Gg.InputName  = {'F1', 'F2', 'F3'};
  Gg.OutputName = {'Ax1', 'Az1', 'Ax2', 'Az2'};
#+end_src

We can now see that the system is unstable due to gravity.
#+begin_src matlab :results output replace :exports results
  pole(Gg)
#+end_src

#+RESULTS:
#+begin_example
pole(Gg)
ans =
          -10.9848275341252 +                     0i
           10.9838836405201 +                     0i
      -7.49855379478109e-05 +      8.65962885770051i
      -7.49855379478109e-05 -      8.65962885770051i
      -6.68819548733559e-06 +     0.832960422243848i
      -6.68819548733559e-06 -     0.832960422243848i
#+end_example

#+begin_src matlab :exports none
  freqs = logspace(-2, 2, 1000);

  figure;
  for in_i = 1:3
      for out_i = 1:4
          subplot(4, 3, 3*(out_i-1)+in_i);
          hold on;
          plot(freqs, abs(squeeze(freqresp(G(out_i,in_i), freqs, 'Hz'))), '-');
          plot(freqs, abs(squeeze(freqresp(Gg(out_i,in_i), freqs, 'Hz'))), '-');
          hold off;
          set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
      end
  end
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/open_loop_tf_g.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:open_loop_tf_g
#+caption: Open Loop Transfer Function from 3 Actuators to 4 Accelerometers with an without gravity
#+RESULTS:
[[file:figs/open_loop_tf_g.png]]

** Analytical Model
*** Parameters
Bode options.
#+begin_src matlab
  P = bodeoptions;
  P.FreqUnits = 'Hz';
  P.MagUnits = 'abs';
  P.MagScale = 'log';
  P.Grid = 'on';
  P.PhaseWrapping = 'on';
  P.Title.FontSize = 14;
  P.XLabel.FontSize = 14;
  P.YLabel.FontSize = 14;
  P.TickLabel.FontSize = 12;
  P.Xlim = [1e-1,1e2];
  P.MagLowerLimMode = 'manual';
  P.MagLowerLim= 1e-3;
#+end_src

Frequency vector.
#+begin_src matlab
  w = 2*pi*logspace(-1,2,1000); % [rad/s]
#+end_src

*** Generation of the State Space Model
Mass matrix
#+begin_src matlab
  M = [m 0 0
       0 m 0
       0 0 I];
#+end_src

Jacobian of the bottom sensor
#+begin_src matlab
  Js1 = [1 0  h/2
         0 1 -l/2];
#+end_src

Jacobian of the top sensor
#+begin_src matlab
  Js2 = [1 0 -h/2
         0 1  0];
#+end_src

Jacobian of the actuators
#+begin_src matlab
  Ja = [1 0  ha   % Left horizontal actuator
        0 1 -la   % Left vertical actuator
        0 1  la]; % Right vertical actuator
  Jta = Ja';
#+end_src

Stiffness and Damping matrices
#+begin_src matlab
  K = k*Jta*Ja;
  C = c*Jta*Ja;
#+end_src

State Space Matrices
#+begin_src matlab
  E = [1 0 0
       0 1 0
       0 0 1]; %projecting ground motion in the directions of the legs

  AA = [zeros(3) eye(3)
        -M\K -M\C];

  BB = [zeros(3,6)
        M\Jta M\(k*Jta*E)];

  CC = [[Js1;Js2] zeros(4,3);
        zeros(2,6)
        (Js1+Js2)./2 zeros(2,3)
        (Js1-Js2)./2 zeros(2,3)
        (Js1-Js2)./(2*h) zeros(2,3)];

  DD = [zeros(4,6)
        zeros(2,3) eye(2,3)
        zeros(6,6)];
#+end_src

State Space model:
- Input = three actuators and three ground motions
- Output = the bottom sensor; the top sensor; the ground motion; the half sum; the half difference; the rotation

#+begin_src matlab
  system_dec = ss(AA,BB,CC,DD);
#+end_src


#+begin_src matlab :results output replace
  size(system_dec)
#+end_src

#+RESULTS:
: State-space model with 12 outputs, 6 inputs, and 6 states.

*** Comparison with the Simscape Model
#+begin_src matlab :exports none
  freqs = logspace(-2, 2, 1000);

  figure;
  for in_i = 1:3
      for out_i = 1:4
          subplot(4, 3, 3*(out_i-1)+in_i);
          hold on;
          plot(freqs, abs(squeeze(freqresp(G(out_i,in_i), freqs, 'Hz'))), '-');
          plot(freqs, abs(squeeze(freqresp(system_dec(out_i,in_i)*s^2, freqs, 'Hz'))), '-');
          hold off;
          set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
      end
  end
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/gravimeter_analytical_system_open_loop_models.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:gravimeter_analytical_system_open_loop_models
#+caption: Comparison of the analytical and the Simscape models
#+RESULTS:
[[file:figs/gravimeter_analytical_system_open_loop_models.png]]

*** Analysis
#+begin_src matlab
  % figure
  % bode(system_dec,P);
  % return
#+end_src

#+begin_src matlab
  %% svd decomposition
  % system_dec_freq = freqresp(system_dec,w);
  % S = zeros(3,length(w));
  % for m = 1:length(w)
  %     S(:,m) = svd(system_dec_freq(1:4,1:3,m));
  % end
  % figure
  % loglog(w./(2*pi), S);hold on;
  % % loglog(w./(2*pi), abs(Val(1,:)),w./(2*pi), abs(Val(2,:)),w./(2*pi), abs(Val(3,:)));
  % xlabel('Frequency [Hz]');ylabel('Singular Value [-]');
  % legend('\sigma_1','\sigma_2','\sigma_3');%,'\sigma_4','\sigma_5','\sigma_6');
  % ylim([1e-8 1e-2]);
  %
  % %condition number
  % figure
  % loglog(w./(2*pi), S(1,:)./S(3,:));hold on;
  % % loglog(w./(2*pi), abs(Val(1,:)),w./(2*pi), abs(Val(2,:)),w./(2*pi), abs(Val(3,:)));
  % xlabel('Frequency [Hz]');ylabel('Condition number [-]');
  % % legend('\sigma_1','\sigma_2','\sigma_3');%,'\sigma_4','\sigma_5','\sigma_6');
  %
  % %performance indicator
  % system_dec_svd = freqresp(system_dec(1:4,1:3),2*pi*10);
  % [U,S,V] = svd(system_dec_svd);
  % H_svd_OL = -eye(3,4);%-[zpk(-2*pi*10,-2*pi*40,40/10) 0 0 0; 0 10*zpk(-2*pi*40,-2*pi*200,40/200) 0 0; 0 0 zpk(-2*pi*2,-2*pi*10,10/2) 0];% - eye(3,4);%
  % H_svd = pinv(V')*H_svd_OL*pinv(U);
  % % system_dec_control_svd_ = feedback(system_dec,g*pinv(V')*H*pinv(U));
  %
  % OL_dec = g_svd*H_svd*system_dec(1:4,1:3);
  % OL_freq = freqresp(OL_dec,w); % OL = G*H
  % CL_system = feedback(eye(3),-g_svd*H_svd*system_dec(1:4,1:3));
  % CL_freq = freqresp(CL_system,w); % CL = (1+G*H)^-1
  % % CL_system_2 = feedback(system_dec,H);
  % % CL_freq_2 = freqresp(CL_system_2,w); % CL = G/(1+G*H)
  % for i = 1:size(w,2)
  %     OL(:,i) = svd(OL_freq(:,:,i));
  %     CL (:,i) = svd(CL_freq(:,:,i));
  %     %CL2 (:,i) = svd(CL_freq_2(:,:,i));
  % end
  %
  % un = ones(1,length(w));
  % figure
  % loglog(w./(2*pi),OL(3,:)+1,'k',w./(2*pi),OL(3,:)-1,'b',w./(2*pi),1./CL(1,:),'r--',w./(2*pi),un,'k:');hold on;%
  % % loglog(w./(2*pi), 1./(CL(2,:)),w./(2*pi), 1./(CL(3,:)));
  % % semilogx(w./(2*pi), 1./(CL2(1,:)),w./(2*pi), 1./(CL2(2,:)),w./(2*pi), 1./(CL2(3,:)));
  % xlabel('Frequency [Hz]');ylabel('Singular Value [-]');
  % legend('GH \sigma_{inf} +1 ','GH \sigma_{inf} -1','S 1/\sigma_{sup}');%,'\lambda_1','\lambda_2','\lambda_3');
  %
  % figure
  % loglog(w./(2*pi),OL(1,:)+1,'k',w./(2*pi),OL(1,:)-1,'b',w./(2*pi),1./CL(3,:),'r--',w./(2*pi),un,'k:');hold on;%
  % % loglog(w./(2*pi), 1./(CL(2,:)),w./(2*pi), 1./(CL(3,:)));
  % % semilogx(w./(2*pi), 1./(CL2(1,:)),w./(2*pi), 1./(CL2(2,:)),w./(2*pi), 1./(CL2(3,:)));
  % xlabel('Frequency [Hz]');ylabel('Singular Value [-]');
  % legend('GH \sigma_{sup} +1 ','GH \sigma_{sup} -1','S 1/\sigma_{inf}');%,'\lambda_1','\lambda_2','\lambda_3');
#+end_src

*** Control Section
#+begin_src matlab
  system_dec_10Hz = freqresp(system_dec,2*pi*10);
  system_dec_0Hz = freqresp(system_dec,0);

  system_decReal_10Hz = pinv(align(system_dec_10Hz));
  [Ureal,Sreal,Vreal] = svd(system_decReal_10Hz(1:4,1:3));
  normalizationMatrixReal = abs(pinv(Ureal)*system_dec_0Hz(1:4,1:3)*pinv(Vreal'));

  [U,S,V] = svd(system_dec_10Hz(1:4,1:3));
  normalizationMatrix = abs(pinv(U)*system_dec_0Hz(1:4,1:3)*pinv(V'));

  H_dec = ([zpk(-2*pi*5,-2*pi*30,30/5) 0 0 0
            0 zpk(-2*pi*4,-2*pi*20,20/4) 0 0
            0 0 0 zpk(-2*pi,-2*pi*10,10)]);
  H_cen_OL = [zpk(-2*pi,-2*pi*10,10) 0 0; 0 zpk(-2*pi,-2*pi*10,10) 0;
              0 0 zpk(-2*pi*5,-2*pi*30,30/5)];
  H_cen = pinv(Jta)*H_cen_OL*pinv([Js1; Js2]);
  % H_svd_OL = -[1/normalizationMatrix(1,1) 0 0 0
  %     0 1/normalizationMatrix(2,2) 0 0
  %     0 0 1/normalizationMatrix(3,3) 0];
  % H_svd_OL_real = -[1/normalizationMatrixReal(1,1) 0 0 0
  %     0 1/normalizationMatrixReal(2,2) 0 0
  %     0 0 1/normalizationMatrixReal(3,3) 0];
  H_svd_OL = -[1/normalizationMatrix(1,1)*zpk(-2*pi*10,-2*pi*60,60/10) 0 0 0
               0 1/normalizationMatrix(2,2)*zpk(-2*pi*5,-2*pi*30,30/5) 0 0
               0 0 1/normalizationMatrix(3,3)*zpk(-2*pi*2,-2*pi*10,10/2) 0];
  H_svd_OL_real = -[1/normalizationMatrixReal(1,1)*zpk(-2*pi*10,-2*pi*60,60/10) 0 0 0
                    0 1/normalizationMatrixReal(2,2)*zpk(-2*pi*5,-2*pi*30,30/5) 0 0
                    0 0 1/normalizationMatrixReal(3,3)*zpk(-2*pi*2,-2*pi*10,10/2) 0];
  % H_svd_OL_real = -[zpk(-2*pi*10,-2*pi*40,40/10) 0 0 0; 0 10*zpk(-2*pi*10,-2*pi*100,100/10) 0 0; 0 0 zpk(-2*pi*2,-2*pi*10,10/2) 0];%-eye(3,4);
  % H_svd_OL = -[zpk(-2*pi*10,-2*pi*40,40/10) 0 0 0; 0 zpk(-2*pi*4,-2*pi*20,4/20) 0 0; 0 0 zpk(-2*pi*2,-2*pi*10,10/2) 0];% - eye(3,4);%
  H_svd = pinv(V')*H_svd_OL*pinv(U);
  H_svd_real = pinv(Vreal')*H_svd_OL_real*pinv(Ureal);

  OL_dec = g*H_dec*system_dec(1:4,1:3);
  OL_cen = g*H_cen_OL*pinv([Js1; Js2])*system_dec(1:4,1:3)*pinv(Jta);
  OL_svd = 100*H_svd_OL*pinv(U)*system_dec(1:4,1:3)*pinv(V');
  OL_svd_real = 100*H_svd_OL_real*pinv(Ureal)*system_dec(1:4,1:3)*pinv(Vreal');
#+end_src

#+begin_src matlab
  % figure
  % bode(OL_dec,w,P);title('OL Decentralized');
  % figure
  % bode(OL_cen,w,P);title('OL Centralized');
#+end_src

#+begin_src matlab
  figure
  bode(g*system_dec(1:4,1:3),w,P);
  title('gain * Plant');
#+end_src

#+begin_src matlab
  figure
  bode(OL_svd,OL_svd_real,w,P);
  title('OL SVD');
  legend('SVD of Complex plant','SVD of real approximation of the complex plant')
#+end_src

#+begin_src matlab
  figure
  bode(system_dec(1:4,1:3),pinv(U)*system_dec(1:4,1:3)*pinv(V'),P);
#+end_src

#+begin_src matlab
  CL_dec = feedback(system_dec,g*H_dec,[1 2 3],[1 2 3 4]);
  CL_cen = feedback(system_dec,g*H_cen,[1 2 3],[1 2 3 4]);
  CL_svd = feedback(system_dec,100*H_svd,[1 2 3],[1 2 3 4]);
  CL_svd_real = feedback(system_dec,100*H_svd_real,[1 2 3],[1 2 3 4]);
#+end_src

#+begin_src matlab
  pzmap_testCL(system_dec,H_dec,g,[1 2 3],[1 2 3 4])
  title('Decentralized control');
#+end_src

#+begin_src matlab
  pzmap_testCL(system_dec,H_cen,g,[1 2 3],[1 2 3 4])
  title('Centralized control');
#+end_src

#+begin_src matlab
  pzmap_testCL(system_dec,H_svd,100,[1 2 3],[1 2 3 4])
  title('SVD control');
#+end_src

#+begin_src matlab
  pzmap_testCL(system_dec,H_svd_real,100,[1 2 3],[1 2 3 4])
  title('Real approximation SVD control');
#+end_src

#+begin_src matlab
  P.Ylim = [1e-8 1e-3];
  figure
  bodemag(system_dec(1:4,1:3),CL_dec(1:4,1:3),CL_cen(1:4,1:3),CL_svd(1:4,1:3),CL_svd_real(1:4,1:3),P);
  title('Motion/actuator')
  legend('Control OFF','Decentralized control','Centralized control','SVD control','SVD control real appr.');
#+end_src

#+begin_src matlab
  P.Ylim = [1e-5 1e1];
  figure
  bodemag(system_dec(1:4,4:6),CL_dec(1:4,4:6),CL_cen(1:4,4:6),CL_svd(1:4,4:6),CL_svd_real(1:4,4:6),P);
  title('Transmissibility');
  legend('Control OFF','Decentralized control','Centralized control','SVD control','SVD control real appr.');
#+end_src

#+begin_src matlab
  figure
  bodemag(system_dec([7 9],4:6),CL_dec([7 9],4:6),CL_cen([7 9],4:6),CL_svd([7 9],4:6),CL_svd_real([7 9],4:6),P);
  title('Transmissibility from half sum and half difference in the X direction');
  legend('Control OFF','Decentralized control','Centralized control','SVD control','SVD control real appr.');
#+end_src

#+begin_src matlab
  figure
  bodemag(system_dec([8 10],4:6),CL_dec([8 10],4:6),CL_cen([8 10],4:6),CL_svd([8 10],4:6),CL_svd_real([8 10],4:6),P);
  title('Transmissibility from half sum and half difference in the Z direction');
  legend('Control OFF','Decentralized control','Centralized control','SVD control','SVD control real appr.');
#+end_src

*** Greshgorin radius
#+begin_src matlab
  system_dec_freq = freqresp(system_dec,w);
  x1 = zeros(1,length(w));
  z1 = zeros(1,length(w));
  x2 = zeros(1,length(w));
  S1 = zeros(1,length(w));
  S2 = zeros(1,length(w));
  S3 = zeros(1,length(w));

  for t = 1:length(w)
      x1(t) = (abs(system_dec_freq(1,2,t))+abs(system_dec_freq(1,3,t)))/abs(system_dec_freq(1,1,t));
      z1(t) = (abs(system_dec_freq(2,1,t))+abs(system_dec_freq(2,3,t)))/abs(system_dec_freq(2,2,t));
      x2(t) = (abs(system_dec_freq(3,1,t))+abs(system_dec_freq(3,2,t)))/abs(system_dec_freq(3,3,t));
      system_svd = pinv(Ureal)*system_dec_freq(1:4,1:3,t)*pinv(Vreal');
      S1(t) = (abs(system_svd(1,2))+abs(system_svd(1,3)))/abs(system_svd(1,1));
      S2(t) = (abs(system_svd(2,1))+abs(system_svd(2,3)))/abs(system_svd(2,2));
      S2(t) = (abs(system_svd(3,1))+abs(system_svd(3,2)))/abs(system_svd(3,3));
  end

  limit = 0.5*ones(1,length(w));
#+end_src

#+begin_src matlab
  figure
  loglog(w./(2*pi),x1,w./(2*pi),z1,w./(2*pi),x2,w./(2*pi),limit,'--');
  legend('x_1','z_1','x_2','Limit');
  xlabel('Frequency [Hz]');
  ylabel('Greshgorin radius [-]');
#+end_src

#+begin_src matlab
  figure
  loglog(w./(2*pi),S1,w./(2*pi),S2,w./(2*pi),S3,w./(2*pi),limit,'--');
  legend('S1','S2','S3','Limit');
  xlabel('Frequency [Hz]');
  ylabel('Greshgorin radius [-]');
  % set(gcf,'color','w')
#+end_src

*** Injecting ground motion in the system to have the output
#+begin_src matlab
  Fr = logspace(-2,3,1e3);
  w=2*pi*Fr*1i;
  %fit of the ground motion data in m/s^2/rtHz
  Fr_ground_x = [0.07 0.1 0.15 0.3 0.7 0.8 0.9 1.2 5 10];
  n_ground_x1 = [4e-7 4e-7 2e-6 1e-6 5e-7 5e-7 5e-7 1e-6 1e-5 3.5e-5];
  Fr_ground_v = [0.07 0.08 0.1 0.11 0.12 0.15 0.25 0.6 0.8 1 1.2 1.6 2 6 10];
  n_ground_v1 = [7e-7 7e-7 7e-7 1e-6 1.2e-6 1.5e-6 1e-6 9e-7 7e-7 7e-7 7e-7 1e-6 2e-6 1e-5 3e-5];

  n_ground_x = interp1(Fr_ground_x,n_ground_x1,Fr,'linear');
  n_ground_v = interp1(Fr_ground_v,n_ground_v1,Fr,'linear');
  % figure
  % loglog(Fr,abs(n_ground_v),Fr_ground_v,n_ground_v1,'*');
  % xlabel('Frequency [Hz]');ylabel('ASD [m/s^2 /rtHz]');
  % return

  %converting into PSD
  n_ground_x = (n_ground_x).^2;
  n_ground_v = (n_ground_v).^2;

  %Injecting ground motion in the system and getting the outputs
  system_dec_f = (freqresp(system_dec,abs(w)));
  PHI = zeros(size(Fr,2),12,12);
  for p = 1:size(Fr,2)
      Sw=zeros(6,6);
      Iact = zeros(3,3);
      Sw(4,4) = n_ground_x(p);
      Sw(5,5) = n_ground_v(p);
      Sw(6,6) = n_ground_v(p);
      Sw(1:3,1:3) = Iact;
      PHI(p,:,:) = (system_dec_f(:,:,p))*Sw(:,:)*(system_dec_f(:,:,p))';
  end
  x1 = PHI(:,1,1);
  z1 = PHI(:,2,2);
  x2 = PHI(:,3,3);
  z2 = PHI(:,4,4);
  wx = PHI(:,5,5);
  wz = PHI(:,6,6);
  x12 = PHI(:,1,3);
  z12 = PHI(:,2,4);
  PHIwx = PHI(:,1,5);
  PHIwz = PHI(:,2,6);
  xsum = PHI(:,7,7);
  zsum = PHI(:,8,8);
  xdelta = PHI(:,9,9);
  zdelta = PHI(:,10,10);
  rot = PHI(:,11,11);
#+end_src

* Gravimeter - Functions
:PROPERTIES:
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
** =align=
:PROPERTIES:
:header-args:matlab+: :tangle gravimeter/align.m
:END:
<<sec:align>>

This Matlab function is accessible [[file:gravimeter/align.m][here]].

#+begin_src matlab
  function [A] = align(V)
  %A!ALIGN(V) returns a constat matrix A which is the real alignment of the
  %INVERSE of the complex input matrix V
  %from Mohit slides

      if (nargin ==0) || (nargin > 1)
          disp('usage: mat_inv_real = align(mat)')
          return
      end

      D = pinv(real(V'*V));
      A = D*real(V'*diag(exp(1i * angle(diag(V*D*V.'))/2)));


  end
#+end_src


** =pzmap_testCL=
:PROPERTIES:
:header-args:matlab+: :tangle gravimeter/pzmap_testCL.m
:END:
<<sec:pzmap_testCL>>

This Matlab function is accessible [[file:gravimeter/pzmap_testCL.m][here]].

#+begin_src matlab
  function [] = pzmap_testCL(system,H,gain,feedin,feedout)
  % evaluate and plot the pole-zero map for the closed loop system for
  % different values of the gain

      [~, n] = size(gain);
      [m1, n1, ~] = size(H);
      [~,n2] = size(feedin);

      figure
      for i = 1:n
          %     if n1 == n2
          system_CL = feedback(system,gain(i)*H,feedin,feedout);

          [P,Z] = pzmap(system_CL);
          plot(real(P(:)),imag(P(:)),'x',real(Z(:)),imag(Z(:)),'o');hold on
          xlabel('Real axis (s^{-1})');ylabel('Imaginary Axis (s^{-1})');
          %         clear P Z
          %     else
          %         system_CL = feedback(system,gain(i)*H(:,1+(i-1)*m1:m1+(i-1)*m1),feedin,feedout);
          %
          %         [P,Z] = pzmap(system_CL);
          %         plot(real(P(:)),imag(P(:)),'x',real(Z(:)),imag(Z(:)),'o');hold on
          %         xlabel('Real axis (s^{-1})');ylabel('Imaginary Axis (s^{-1})');
          %         clear P Z
          %     end
      end
      str = {strcat('gain = ' , num2str(gain(1)))};  % at the end of first loop, z being loop output
      str = [str , strcat('gain = ' , num2str(gain(1)))]; % after 2nd loop
      for i = 2:n
          str = [str , strcat('gain = ' , num2str(gain(i)))]; % after 2nd loop
          str = [str , strcat('gain = ' , num2str(gain(i)))]; % after 2nd loop
      end
      legend(str{:})
  end

#+end_src

* Stewart Platform - Simscape Model
:PROPERTIES:
:header-args:matlab+: :tangle stewart_platform/simscape_model.m
:END:
** Introduction                                                      :ignore:

In this analysis, we wish to applied SVD control to the Stewart Platform shown in Figure [[fig:SP_assembly]].

#+name: fig:SP_assembly
#+caption: Stewart Platform CAD View
[[file:figs/SP_assembly.png]]

The analysis of the SVD control applied to the Stewart platform is performed in the following sections:
- Section [[sec:stewart_simscape]]: The parameters of the Simscape model of the Stewart platform are defined
- Section [[sec:stewart_identification]]: The plant is identified from the Simscape model and the centralized plant is computed thanks to the Jacobian
- Section [[sec:stewart_dynamics]]: The identified Dynamics is shown
- Section [[sec:stewart_real_approx]]: A real approximation of the plant is computed for further decoupling using the Singular Value Decomposition (SVD)
- Section [[sec:stewart_svd_decoupling]]: The decoupling is performed thanks to the SVD. The effectiveness of the decoupling is verified using the Gershorin Radii
- Section [[sec:stewart_decoupled_plant]]: The dynamics of the decoupled plant is shown
- Section [[sec:stewart_diagonal_control]]: A diagonal controller is defined to control the decoupled plant
- Section [[sec:stewart_closed_loop_results]]: Finally, the closed loop system properties are studied

** Matlab Init                                             :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
#+end_src

#+begin_src matlab :tangle no
  addpath('stewart_platform');
  addpath('stewart_platform/STEP');
#+end_src

#+begin_src matlab :eval no
  addpath('STEP');
#+end_src

** Jacobian                                                        :noexport:
First, the position of the "joints" (points of force application) are estimated and the Jacobian computed.
#+begin_src matlab
  open('drone_platform_jacobian.slx');
#+end_src

#+begin_src matlab
  sim('drone_platform_jacobian');
#+end_src

#+begin_src matlab
  Aa = [a1.Data(1,:);
        a2.Data(1,:);
        a3.Data(1,:);
        a4.Data(1,:);
        a5.Data(1,:);
        a6.Data(1,:)]';

  Ab = [b1.Data(1,:);
        b2.Data(1,:);
        b3.Data(1,:);
        b4.Data(1,:);
        b5.Data(1,:);
        b6.Data(1,:)]';

  As = (Ab - Aa)./vecnorm(Ab - Aa);

  l = vecnorm(Ab - Aa)';

  J = [As' , cross(Ab, As)'];

  save('./jacobian.mat', 'Aa', 'Ab', 'As', 'l', 'J');
#+end_src

** Simscape Model - Parameters
<<sec:stewart_simscape>>
#+begin_src matlab
  open('drone_platform.slx');
#+end_src

Definition of spring parameters
#+begin_src matlab
  kx = 0.5*1e3/3; % [N/m]
  ky = 0.5*1e3/3;
  kz = 1e3/3;

  cx = 0.025; % [Nm/rad]
  cy = 0.025;
  cz = 0.025;
#+end_src

Gravity:
#+begin_src matlab
  g = 0;
#+end_src

We load the Jacobian (previously computed from the geometry).
#+begin_src matlab
  load('./jacobian.mat', 'Aa', 'Ab', 'As', 'l', 'J');
#+end_src

** Identification of the plant
<<sec:stewart_identification>>

The dynamics is identified from forces applied by each legs to the measured acceleration of the top platform.
#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'drone_platform';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Dw'],              1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/u'],               1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Inertial Sensor'], 1, 'openoutput'); io_i = io_i + 1;

  G = linearize(mdl, io);
  G.InputName  = {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz', ...
                  'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
  G.OutputName = {'Ax', 'Ay', 'Az', 'Arx', 'Ary', 'Arz'};
#+end_src

There are 24 states (6dof for the bottom platform + 6dof for the top platform).
#+begin_src matlab :results output replace
  size(G)
#+end_src

#+RESULTS:
: State-space model with 6 outputs, 12 inputs, and 24 states.

The "centralized" plant $\bm{G}_x$ is now computed (Figure [[fig:centralized_control]]).

#+name: fig:centralized_control
#+caption: Centralized control architecture
[[file:figs/centralized_control.png]]

Thanks to the Jacobian, we compute the transfer functions in the inertial frame (transfer function from forces and torques applied to the top platform to the absolute acceleration of the top platform).
#+begin_src matlab
  Gx = G*blkdiag(eye(6), inv(J'));
  Gx.InputName  = {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz', ...
                   'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
#+end_src

** Obtained Dynamics
<<sec:stewart_dynamics>>

#+begin_src matlab :exports none
  freqs = logspace(-1, 2, 1000);

  figure;
  tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

  % Magnitude
  ax1 = nexttile([2, 1]);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gx('Ax', 'Fx'), freqs, 'Hz'))), 'DisplayName', '$A_x/F_x$');
  plot(freqs, abs(squeeze(freqresp(Gx('Ay', 'Fy'), freqs, 'Hz'))), 'DisplayName', '$A_y/F_y$');
  plot(freqs, abs(squeeze(freqresp(Gx('Az', 'Fz'), freqs, 'Hz'))), 'DisplayName', '$A_z/F_z$');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
  legend('location', 'southeast');

  % Phase
  ax2 = nexttile;
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ax', 'Fx'), freqs, 'Hz'))));
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ay', 'Fy'), freqs, 'Hz'))));
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Az', 'Fz'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-180, 180]);
  yticks([-360:90:360]);

  linkaxes([ax1,ax2],'x');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/stewart_platform_translations.pdf', 'eps', true, 'width', 'wide', 'height', 'tall');
#+end_src

#+name: fig:stewart_platform_translations
#+caption: Stewart Platform Plant from forces applied by the legs to the acceleration of the platform
#+RESULTS:
[[file:figs/stewart_platform_translations.png]]

#+begin_src matlab :exports none
  freqs = logspace(-1, 2, 1000);

  figure;
  tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

  % Magnitude
  ax1 = nexttile([2, 1]);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gx('Arx', 'Mx'), freqs, 'Hz'))), 'DisplayName', '$A_{R_x}/M_x$');
  plot(freqs, abs(squeeze(freqresp(Gx('Ary', 'My'), freqs, 'Hz'))), 'DisplayName', '$A_{R_y}/M_y$');
  plot(freqs, abs(squeeze(freqresp(Gx('Arz', 'Mz'), freqs, 'Hz'))), 'DisplayName', '$A_{R_z}/M_z$');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Magnitude [rad/(Nm)]'); set(gca, 'XTickLabel',[]);
  legend('location', 'southeast');

  % Phase
  ax2 = nexttile;
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Arx', 'Mx'), freqs, 'Hz'))));
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ary', 'My'), freqs, 'Hz'))));
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Arz', 'Mz'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-180, 180]);
  yticks([-360:90:360]);

  linkaxes([ax1,ax2],'x');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/stewart_platform_rotations.pdf', 'eps', true, 'width', 'wide', 'height', 'tall');
#+end_src

#+name: fig:stewart_platform_rotations
#+caption: Stewart Platform Plant from torques applied by the legs to the angular acceleration of the platform
#+RESULTS:
[[file:figs/stewart_platform_rotations.png]]

** Real Approximation of $G$ at the decoupling frequency
<<sec:stewart_real_approx>>

Let's compute a real approximation of the complex matrix $H_1$ which corresponds to the the transfer function $G_c(j\omega_c)$ from forces applied by the actuators to the measured acceleration of the top platform evaluated at the frequency $\omega_c$.
#+begin_src matlab
  wc = 2*pi*30; % Decoupling frequency [rad/s]

  Gc = G({'Ax', 'Ay', 'Az', 'Arx', 'Ary', 'Arz'}, ...
         {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}); % Transfer function to find a real approximation

  H1 = evalfr(Gc, j*wc);
#+end_src

The real approximation is computed as follows:
#+begin_src matlab
  D = pinv(real(H1'*H1));
  H1 = inv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no
  data2orgtable(H1, {}, {}, ' %.1f ');
#+end_src

#+caption: Real approximate of $G$ at the decoupling frequency $\omega_c$
#+RESULTS:
|    4.4 |   -2.1 |   -2.1 |    4.4 |  -2.4 |   -2.4 |
|   -0.2 |   -3.9 |    3.9 |    0.2 |  -3.8 |    3.8 |
|    3.4 |    3.4 |    3.4 |    3.4 |   3.4 |    3.4 |
| -367.1 | -323.8 |  323.8 |  367.1 |  43.3 |  -43.3 |
| -162.0 | -237.0 | -237.0 | -162.0 | 398.9 |  398.9 |
|  220.6 | -220.6 |  220.6 | -220.6 | 220.6 | -220.6 |


Note that the plant $G$ at $\omega_c$ is already an almost real matrix.
This can be seen on the Bode plots where the phase is close to 1.
This can be verified below where only the real value of $G(\omega_c)$ is shown

#+begin_src matlab :exports results :results value table replace :tangle no
  data2orgtable(real(evalfr(Gc, j*wc)), {}, {}, ' %.1f ');
#+end_src

#+RESULTS:
|    4.4 |   -2.1 |   -2.1 |    4.4 |  -2.4 |   -2.4 |
|   -0.2 |   -3.9 |    3.9 |    0.2 |  -3.8 |    3.8 |
|    3.4 |    3.4 |    3.4 |    3.4 |   3.4 |    3.4 |
| -367.1 | -323.8 |  323.8 |  367.1 |  43.3 |  -43.3 |
| -162.0 | -237.0 | -237.0 | -162.0 | 398.9 |  398.9 |
|  220.6 | -220.6 |  220.6 | -220.6 | 220.6 | -220.6 |

** Verification of the decoupling using the "Gershgorin Radii"
<<sec:stewart_svd_decoupling>>

First, the Singular Value Decomposition of $H_1$ is performed:
\[ H_1 = U \Sigma V^H \]

#+begin_src matlab
  [U,S,V] = svd(H1);
#+end_src

Then, the "Gershgorin Radii" is computed for the plant $G_c(s)$ and the "SVD Decoupled Plant" $G_d(s)$:
\[ G_d(s) = U^T G_c(s) V \]

This is computed over the following frequencies.
#+begin_src matlab
  freqs = logspace(-2, 2, 1000); % [Hz]
#+end_src

Gershgorin Radii for the coupled plant:
#+begin_src matlab
  Gr_coupled = zeros(length(freqs), size(Gc,2));

  H = abs(squeeze(freqresp(Gc, freqs, 'Hz')));
  for out_i = 1:size(Gc,2)
      Gr_coupled(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :));
  end
#+end_src

Gershgorin Radii for the decoupled plant using SVD:
#+begin_src matlab
  Gd = U'*Gc*V;
  Gr_decoupled = zeros(length(freqs), size(Gd,2));

  H = abs(squeeze(freqresp(Gd, freqs, 'Hz')));
  for out_i = 1:size(Gd,2)
      Gr_decoupled(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :));
  end
#+end_src

Gershgorin Radii for the decoupled plant using the Jacobian:
#+begin_src matlab
  Gj = Gc*inv(J');
  Gr_jacobian = zeros(length(freqs), size(Gj,2));

  H = abs(squeeze(freqresp(Gj, freqs, 'Hz')));

  for out_i = 1:size(Gj,2)
      Gr_jacobian(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :));
  end
#+end_src

#+begin_src matlab :exports results
  figure;
  hold on;
  plot(freqs, Gr_coupled(:,1), 'DisplayName', 'Coupled');
  plot(freqs, Gr_decoupled(:,1), 'DisplayName', 'SVD');
  plot(freqs, Gr_jacobian(:,1), 'DisplayName', 'Jacobian');
  for in_i = 2:6
      set(gca,'ColorOrderIndex',1)
      plot(freqs, Gr_coupled(:,in_i), 'HandleVisibility', 'off');
      set(gca,'ColorOrderIndex',2)
      plot(freqs, Gr_decoupled(:,in_i), 'HandleVisibility', 'off');
      set(gca,'ColorOrderIndex',3)
      plot(freqs, Gr_jacobian(:,in_i), 'HandleVisibility', 'off');
  end
  plot(freqs, 0.5*ones(size(freqs)), 'k--', 'DisplayName', 'Limit')
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  hold off;
  xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
  legend('location', 'northeast');
  ylim([1e-3, 1e3]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/simscape_model_gershgorin_radii.pdf', 'eps', true, 'width', 'wide', 'height', 'tall');
#+end_src

#+name: fig:simscape_model_gershgorin_radii
#+caption: Gershgorin Radii of the Coupled and Decoupled plants
#+RESULTS:
[[file:figs/simscape_model_gershgorin_radii.png]]

** Decoupled Plant
<<sec:stewart_decoupled_plant>>

Let's see the bode plot of the decoupled plant $G_d(s)$.
\[ G_d(s) = U^T G_c(s) V \]

#+begin_src matlab :exports results
  freqs = logspace(-1, 2, 1000);

  figure;
  hold on;
  for ch_i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gd(ch_i, ch_i), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$G(%i, %i)$', ch_i, ch_i));
  end
  for in_i = 1:5
    for out_i = in_i+1:6
      plot(freqs, abs(squeeze(freqresp(Gd(out_i, in_i), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
           'HandleVisibility', 'off');
    end
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Magnitude'); xlabel('Frequency [Hz]');
  legend('location', 'northwest');
  ylim([1e-3, 1e4]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/simscape_model_decoupled_plant_svd.pdf', 'eps', true, 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:simscape_model_decoupled_plant_svd
#+caption: Decoupled Plant using SVD
#+RESULTS:
[[file:figs/simscape_model_decoupled_plant_svd.png]]

#+begin_src matlab :exports results
  freqs = logspace(-1, 2, 1000);

  figure;
  hold on;
  for ch_i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gj(ch_i, ch_i), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$G(%i, %i)$', ch_i, ch_i));
  end
  for in_i = 1:5
    for out_i = in_i+1:6
      plot(freqs, abs(squeeze(freqresp(Gj(out_i, in_i), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
           'HandleVisibility', 'off');
    end
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Magnitude'); xlabel('Frequency [Hz]');
  legend('location', 'northwest');
  ylim([1e-1, 1e6]);
  set(gca, 'YMinorTick', 'on');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/simscape_model_decoupled_plant_jacobian.pdf', 'eps', true, 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:simscape_model_decoupled_plant_jacobian
#+caption: Decoupled Plant using the Jacobian
#+RESULTS:
[[file:figs/simscape_model_decoupled_plant_jacobian.png]]

** Diagonal Controller
<<sec:stewart_diagonal_control>>

The controller $K$ is a diagonal controller consisting a low pass filters with a crossover frequency $\omega_c$ and a DC gain $C_g$.

#+begin_src matlab
  wc = 2*pi*0.1; % Crossover Frequency [rad/s]
  C_g = 50; % DC Gain

  K = eye(6)*C_g/(s+wc);
#+end_src

The control diagram for the centralized control is shown in Figure [[fig:centralized_control]].

The controller $K_c$ is "working" in an cartesian frame.
The Jacobian is used to convert forces in the cartesian frame to forces applied by the actuators.

#+begin_src latex :file centralized_control.pdf :tangle no :exports results
  \begin{tikzpicture}
    \node[block={2cm}{1.5cm}] (G) {$G$};
    \node[block, below right=0.6 and -0.5 of G] (K) {$K_c$};
    \node[block, below left= 0.6 and -0.5 of G] (J) {$J^{-T}$};

    % Inputs of the controllers
    \coordinate[] (inputd) at ($(G.south west)!0.75!(G.north west)$);
    \coordinate[] (inputu) at ($(G.south west)!0.25!(G.north west)$);

    % Connections and labels
    \draw[<-] (inputd) -- ++(-0.8, 0) node[above right]{$D_w$};
    \draw[->] (G.east) -- ++(2.0, 0)  node[above left]{$a$};
    \draw[->] ($(G.east)+(1.4, 0)$)node[branch]{} |- (K.east);
    \draw[->] (K.west) -- (J.east) node[above right]{$\mathcal{F}$};
    \draw[->] (J.west) -- ++(-0.6, 0) |- (inputu) node[above left]{$\tau$};
  \end{tikzpicture}
#+end_src

#+name: fig:centralized_control
#+caption: Control Diagram for the Centralized control
#+RESULTS:
[[file:figs/centralized_control.png]]

The feedback system is computed as shown below.
#+begin_src matlab
  G_cen = feedback(G, inv(J')*K, [7:12], [1:6]);
#+end_src

The SVD control architecture is shown in Figure [[fig:svd_control]].
The matrices $U$ and $V$ are used to decoupled the plant $G$.
#+begin_src latex :file svd_control.pdf :tangle no :exports results
  \begin{tikzpicture}
    \node[block={2cm}{1.5cm}] (G) {$G$};
    \node[block, below right=0.6 and 0 of G] (U) {$U^{-1}$};
    \node[block, below=0.6 of G] (K) {$K_{\text{SVD}}$};
    \node[block, below left= 0.6 and 0 of G] (V) {$V^{-T}$};

    % Inputs of the controllers
    \coordinate[] (inputd) at ($(G.south west)!0.75!(G.north west)$);
    \coordinate[] (inputu) at ($(G.south west)!0.25!(G.north west)$);

    % Connections and labels
    \draw[<-] (inputd) -- ++(-0.8, 0) node[above right]{$D_w$};
    \draw[->] (G.east) -- ++(2.5, 0) node[above left]{$a$};
    \draw[->] ($(G.east)+(2.0, 0)$) node[branch]{} |- (U.east);
    \draw[->] (U.west) -- (K.east);
    \draw[->] (K.west) -- (V.east);
    \draw[->] (V.west) -- ++(-0.6, 0) |- (inputu) node[above left]{$\tau$};
  \end{tikzpicture}
#+end_src

#+name: fig:svd_control
#+caption: Control Diagram for the SVD control
#+RESULTS:
[[file:figs/svd_control.png]]

The feedback system is computed as shown below.
#+begin_src matlab
  G_svd = feedback(G, pinv(V')*K*pinv(U), [7:12], [1:6]);
#+end_src

** Closed-Loop system Performances
<<sec:stewart_closed_loop_results>>

Let's first verify the stability of the closed-loop systems:
#+begin_src matlab :results output replace text
  isstable(G_cen)
#+end_src

#+RESULTS:
: ans =
:   logical
:    1

#+begin_src matlab :results output replace text
  isstable(G_svd)
#+end_src

#+RESULTS:
: ans =
:   logical
:    0

The obtained transmissibility in Open-loop, for the centralized control as well as for the SVD control are shown in Figure [[fig:stewart_platform_simscape_cl_transmissibility]].

#+begin_src matlab :exports results
  freqs = logspace(-2, 2, 1000);

  figure;
  tiledlayout(2, 2, 'TileSpacing', 'None', 'Padding', 'None');

  ax1 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G(    'Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Open-Loop');
  plot(freqs, abs(squeeze(freqresp(G_cen('Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Centralized');
  plot(freqs, abs(squeeze(freqresp(G_svd('Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', 'SVD');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('$D_x/D_{w,x}$, $D_y/D_{w, y}$'); set(gca, 'XTickLabel',[]);
  legend('location', 'southwest');

  % ax2 = nexttile;
  % hold on;
  % plot(freqs, abs(squeeze(freqresp(G(    'Ay', 'Dwy')/s^2, freqs, 'Hz'))));
  % plot(freqs, abs(squeeze(freqresp(G_cen('Ay', 'Dwy')/s^2, freqs, 'Hz'))));
  % plot(freqs, abs(squeeze(freqresp(G_svd('Ay', 'Dwy')/s^2, freqs, 'Hz'))));
  % hold off;
  % set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  % ylabel('Transmissibility - $D_y/D_{w,y}$');  xlabel('Frequency [Hz]');

  ax3 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G(    'Az', 'Dwz')/s^2, freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(G_cen('Az', 'Dwz')/s^2, freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(G_svd('Az', 'Dwz')/s^2, freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('$D_z/D_{w,z}$'); set(gca, 'XTickLabel',[]);

  ax4 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G(    'Arx', 'Rwx')/s^2, freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(G_cen('Arx', 'Rwx')/s^2, freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(G_svd('Arx', 'Rwx')/s^2, freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('$R_x/R_{w,x}$, $R_y/R_{w,y}$');  xlabel('Frequency [Hz]');

  % ax5 = nexttile;
  % hold on;
  % plot(freqs, abs(squeeze(freqresp(G(    'Ary', 'Rwy')/s^2, freqs, 'Hz'))));
  % plot(freqs, abs(squeeze(freqresp(G_cen('Ary', 'Rwy')/s^2, freqs, 'Hz'))));
  % plot(freqs, abs(squeeze(freqresp(G_svd('Ary', 'Rwy')/s^2, freqs, 'Hz'))));
  % hold off;
  % set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  % ylabel('Transmissibility - $R_y/R_{w,y}$');  xlabel('Frequency [Hz]');

  ax6 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G(    'Arz', 'Rwz')/s^2, freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(G_cen('Arz', 'Rwz')/s^2, freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(G_svd('Arz', 'Rwz')/s^2, freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('$R_z/R_{w,z}$');  xlabel('Frequency [Hz]');

  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
  xlim([freqs(1), freqs(end)]);
  ylim([1e-5, 1e2]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/stewart_platform_simscape_cl_transmissibility.pdf', 'eps', true, 'width', 'wide', 'height', 'tall');
#+end_src

#+name: fig:stewart_platform_simscape_cl_transmissibility
#+caption: Obtained Transmissibility
#+RESULTS:
[[file:figs/stewart_platform_simscape_cl_transmissibility.png]]

* Stewart Platform - Analytical Model                               :noexport:
:PROPERTIES:
:header-args:matlab+: :tangle stewart_platform/analytical_model.m
:END:
** Matlab Init                                              :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
#+end_src

#+begin_src matlab
  %% Bode plot options
  opts = bodeoptions('cstprefs');
  opts.FreqUnits = 'Hz';
  opts.MagUnits = 'abs';
  opts.MagScale = 'log';
  opts.PhaseWrapping = 'on';
  opts.xlim = [1 1000];
#+end_src

** Characteristics
#+begin_src matlab
  L  = 0.055; % Leg length [m]
  Zc = 0;     % ?
  m  = 0.2;   % Top platform mass [m]
  k  = 1e3;   % Total vertical stiffness [N/m]
  c  = 2*0.1*sqrt(k*m); % Damping ? [N/(m/s)]

  Rx = 0.04; % ?
  Rz = 0.04; % ?
  Ix = m*Rx^2; % ?
  Iy = m*Rx^2; % ?
  Iz = m*Rz^2; % ?
#+end_src

** Mass Matrix
#+begin_src matlab
  M = m*[1   0 0  0         Zc        0;
         0   1 0 -Zc        0         0;
         0   0 1  0         0         0;
         0 -Zc 0  Rx^2+Zc^2 0         0;
         Zc  0 0  0         Rx^2+Zc^2 0;
         0   0 0  0         0         Rz^2];
#+end_src

** Jacobian Matrix
#+begin_src matlab
  Bj=1/sqrt(6)*[ 1             1          -2          1         1        -2;
                 sqrt(3)      -sqrt(3)     0          sqrt(3)  -sqrt(3)   0;
                 sqrt(2)       sqrt(2)     sqrt(2)    sqrt(2)   sqrt(2)   sqrt(2);
                 0             0           L          L        -L         -L;
                 -L*2/sqrt(3) -L*2/sqrt(3) L/sqrt(3)  L/sqrt(3) L/sqrt(3)  L/sqrt(3);
                 L*sqrt(2)    -L*sqrt(2)   L*sqrt(2) -L*sqrt(2) L*sqrt(2) -L*sqrt(2)];
#+end_src

** Stifnness and Damping matrices
#+begin_src matlab
  kv = k/3;     % Vertical Stiffness of the springs [N/m]
  kh = 0.5*k/3; % Horizontal Stiffness of the springs [N/m]

  K = diag([3*kh, 3*kh, 3*kv, 3*kv*Rx^2/2, 3*kv*Rx^2/2, 3*kh*Rx^2]); % Stiffness Matrix
  C = c*K/100000; % Damping Matrix
#+end_src

** State Space System
#+begin_src matlab
  A  = [ zeros(6) eye(6); ...
        -M\K     -M\C];
  Bw = [zeros(6); -eye(6)];
  Bu = [zeros(6); M\Bj];

  Co = [-M\K -M\C];
 
  D  = [zeros(6) M\Bj];
 
  ST = ss(A,[Bw Bu],Co,D);
#+end_src

- OUT 1-6: 6 dof
- IN 1-6 : ground displacement in the directions of the legs
- IN 7-12: forces in the actuators.
#+begin_src matlab
  ST.StateName = {'x';'y';'z';'theta_x';'theta_y';'theta_z';...
                  'dx';'dy';'dz';'dtheta_x';'dtheta_y';'dtheta_z'};

  ST.InputName = {'w1';'w2';'w3';'w4';'w5';'w6';...
                  'u1';'u2';'u3';'u4';'u5';'u6'};

  ST.OutputName = {'ax';'ay';'az';'atheta_x';'atheta_y';'atheta_z'};
#+end_src

** Transmissibility
#+begin_src matlab
  TR=ST*[eye(6); zeros(6)];
#+end_src

#+begin_src matlab
  figure
  subplot(231)
  bodemag(TR(1,1));
  subplot(232)
  bodemag(TR(2,2));
  subplot(233)
  bodemag(TR(3,3));
  subplot(234)
  bodemag(TR(4,4));
  subplot(235)
  bodemag(TR(5,5));
  subplot(236)
  bodemag(TR(6,6));
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/stewart_platform_analytical_transmissibility.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:stewart_platform_analytical_transmissibility
#+caption: Transmissibility
#+RESULTS:
[[file:figs/stewart_platform_analytical_transmissibility.png]]

** Real approximation of $G(j\omega)$ at decoupling frequency
#+begin_src matlab
  sys1 = ST*[zeros(6); eye(6)]; % take only the forces inputs

  dec_fr = 20;
  H1 = evalfr(sys1,j*2*pi*dec_fr);
  H2 = H1;
  D = pinv(real(H2'*H2));
  H1 = inv(D*real(H2'*diag(exp(j*angle(diag(H2*D*H2.'))/2)))) ;
  [U,S,V] = svd(H1);

  wf = logspace(-1,2,1000);
  for i  = 1:length(wf)
      H = abs(evalfr(sys1,j*2*pi*wf(i)));
      H_dec = abs(evalfr(U'*sys1*V,j*2*pi*wf(i)));
      for j = 1:size(H,2)
          g_r1(i,j) =  (sum(H(j,:))-H(j,j))/H(j,j);
          g_r2(i,j) =  (sum(H_dec(j,:))-H_dec(j,j))/H_dec(j,j);
          %     keyboard
      end
      g_lim(i) = 0.5;
  end
#+end_src

** Coupled and Decoupled Plant "Gershgorin Radii"
#+begin_src matlab
  figure;
  title('Coupled plant')
  loglog(wf,g_r1(:,1),wf,g_r1(:,2),wf,g_r1(:,3),wf,g_r1(:,4),wf,g_r1(:,5),wf,g_r1(:,6),wf,g_lim,'--');
  legend('$a_x$','$a_y$','$a_z$','$\theta_x$','$\theta_y$','$\theta_z$','Limit');
  xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/gershorin_raddii_coupled_analytical.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:gershorin_raddii_coupled_analytical
#+caption: Gershorin Raddi for the coupled plant
#+RESULTS:
[[file:figs/gershorin_raddii_coupled_analytical.png]]

#+begin_src matlab
  figure;
  title('Decoupled plant (10 Hz)')
  loglog(wf,g_r2(:,1),wf,g_r2(:,2),wf,g_r2(:,3),wf,g_r2(:,4),wf,g_r2(:,5),wf,g_r2(:,6),wf,g_lim,'--');
  legend('$S_1$','$S_2$','$S_3$','$S_4$','$S_5$','$S_6$','Limit');
  xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/gershorin_raddii_decoupled_analytical.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:gershorin_raddii_decoupled_analytical
#+caption: Gershorin Raddi for the decoupled plant
#+RESULTS:
[[file:figs/gershorin_raddii_decoupled_analytical.png]]

** Decoupled Plant
#+begin_src matlab
  figure;
  bodemag(U'*sys1*V,opts)
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/stewart_platform_analytical_decoupled_plant.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:stewart_platform_analytical_decoupled_plant
#+caption: Decoupled Plant
#+RESULTS:
[[file:figs/stewart_platform_analytical_decoupled_plant.png]]

** Controller
#+begin_src matlab
  fc = 2*pi*0.1; % Crossover Frequency [rad/s]
  c_gain = 50; %

  cont = eye(6)*c_gain/(s+fc);
#+end_src

** Closed Loop System
#+begin_src matlab
  FEEDIN  = [7:12]; % Input of controller
  FEEDOUT = [1:6]; % Output of controller
#+end_src

Centralized Control
#+begin_src matlab
  STcen = feedback(ST, inv(Bj)*cont, FEEDIN, FEEDOUT);
  TRcen = STcen*[eye(6); zeros(6)];
#+end_src

SVD Control
#+begin_src matlab
  STsvd = feedback(ST, pinv(V')*cont*pinv(U), FEEDIN, FEEDOUT);
  TRsvd = STsvd*[eye(6); zeros(6)];
#+end_src

** Results
#+begin_src matlab
  figure
  subplot(231)
  bodemag(TR(1,1),TRcen(1,1),TRsvd(1,1),opts)
  legend('OL','Centralized','SVD')
  subplot(232)
  bodemag(TR(2,2),TRcen(2,2),TRsvd(2,2),opts)
  legend('OL','Centralized','SVD')
  subplot(233)
  bodemag(TR(3,3),TRcen(3,3),TRsvd(3,3),opts)
  legend('OL','Centralized','SVD')
  subplot(234)
  bodemag(TR(4,4),TRcen(4,4),TRsvd(4,4),opts)
  legend('OL','Centralized','SVD')
  subplot(235)
  bodemag(TR(5,5),TRcen(5,5),TRsvd(5,5),opts)
  legend('OL','Centralized','SVD')
  subplot(236)
  bodemag(TR(6,6),TRcen(6,6),TRsvd(6,6),opts)
  legend('OL','Centralized','SVD')
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/stewart_platform_analytical_svd_cen_comp.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:stewart_platform_analytical_svd_cen_comp
#+caption: Comparison of the obtained transmissibility for the centralized control and the SVD control
#+RESULTS:
[[file:figs/stewart_platform_analytical_svd_cen_comp.png]]
