%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');

% Run meas_transformation.m script in order to get the tranformation (jacobian) matrix
run('meas_transformation.m')

% Open the Simulink File
open('vibration_table')

% Springs
% <<sec:simscape_springs>>

% The 4 springs supporting the suspended optical table are modelled with "bushing joints" having stiffness and damping in the x-y-z directions:


spring.kx = 1e4; % X- Stiffness [N/m]
spring.cx = 1e1; % X- Damping [N/(m/s)]

spring.ky = 1e4; % Y- Stiffness [N/m]
spring.cy = 1e1; % Y- Damping [N/(m/s)]

spring.kz = 1e4; % Z- Stiffness [N/m]
spring.cz = 1e1; % Z- Damping [N/(m/s)]

spring.z0 = 32e-3; % Equilibrium z-length [m]

% Inertial Shaker (IS20)
% <<sec:simscape_inertial_shaker>>

% The inertial shaker is defined as two solid bodies:
% - the "housing" that is fixed to the element that we want to excite
% - the "inertial mass" that is suspended inside the housing

% The inertial mass is guided inside the housing and an actuator (coil and magnet) can be used to apply a force between the inertial mass and the support.
% The "reacting" force on the support is then used as an excitation.

% #+name: tab:is20_characteristics
% #+caption: Summary of the IS20 datasheet
% #+attr_latex: :environment tabularx :width 0.4\linewidth :align lX
% #+attr_latex: :center t :booktabs t :float t
% | Characteristic  | Value      |
% |-----------------+------------|
% | Output Force    | 20 N       |
% | Frequency Range | 10-3000 Hz |
% | Moving Mass     | 0.1 kg     |
% | Total Mass      | 0.3 kg     |

% From the datasheet in Table [[tab:is20_characteristics]], we can estimate the parameters of the physical shaker.

% These parameters are defined below

shaker.w0 = 2*pi*10; % Resonance frequency of moving mass [rad/s]
shaker.m = 0.1; % Moving mass [m]
shaker.m_tot = 0.3; % Total mass [m]
shaker.k = shaker.m*shaker.w0^2; % Spring constant [N/m]
shaker.c = 0.2*sqrt(shaker.k*shaker.m); % Damping [N/(m/s)]

% 3D accelerometer (356B18)
% <<sec:simscape_accelerometers>>

% An accelerometer consists of 2 solids:
% - a "housing" rigidly fixed to the measured body
% - an "inertial mass" suspended inside the housing by springs and guided in the measured direction

% The relative motion between the housing and the inertial mass gives a measurement of the acceleration of the measured body (up to the suspension mode of the inertial mass).

% #+name: tab:356b18_characteristics
% #+caption: Summary of the 356B18 datasheet
% #+attr_latex: :environment tabularx :width 0.5\linewidth :align lX
% #+attr_latex: :center t :booktabs t :float t
% | Characteristic      | Value               |
% |---------------------+---------------------|
% | Sensitivity         | 0.102 V/(m/s2)      |
% | Frequency Range     | 0.5 to 3000 Hz      |
% | Resonance Frequency | > 20 kHz            |
% | Resolution          | 0.0005 m/s2 rms     |
% | Weight              | 0.025 kg            |
% | Size                | 20.3x26.1x20.3 [mm] |

% Here are defined the parameters for the triaxial accelerometer:

acc_3d.m = 0.005; % Inertial mass [kg]
acc_3d.m_tot = 0.025; % Total mass [m]

acc_3d.w0 = 2*pi*20e3; % Resonance frequency [rad/s]

acc_3d.kx = acc_3d.m*acc_3d.w0^2; % Spring constant [N/m]
acc_3d.ky = acc_3d.m*acc_3d.w0^2; % Spring constant [N/m]
acc_3d.kz = acc_3d.m*acc_3d.w0^2; % Spring constant [N/m]

acc_3d.cx = 1e2; % Damping [N/(m/s)]
acc_3d.cy = 1e2; % Damping [N/(m/s)]
acc_3d.cz = 1e2; % Damping [N/(m/s)]



% DC gain between support acceleration and inertial mass displacement is $-m/k$:

acc_3d.g_x = 1/(-acc_3d.m/acc_3d.kx); % [m/s^2/m]
acc_3d.g_y = 1/(-acc_3d.m/acc_3d.ky); % [m/s^2/m]
acc_3d.g_z = 1/(-acc_3d.m/acc_3d.kz); % [m/s^2/m]



% We also define the sensitivity in order to have the outputs in volts.

acc_3d.gV_x = 0.102; % [V/(m/s^2)]
acc_3d.gV_y = 0.102; % [V/(m/s^2)]
acc_3d.gV_z = 0.102; % [V/(m/s^2)]



% The problem with using such model for accelerometers is that this adds states to the identified models (2x3 states for each triaxial accelerometer).
% These states represents the dynamics of the suspended inertial mass.
% In the frequency band of interest (few Hz up to ~1 kHz), the dynamics of the inertial mass can be ignore (its resonance is way above 1kHz).
% Therefore, we might as well use idealized "transform sensors" blocks as they will give the same result up to ~20kHz while allowing to reduce the number of identified states.

% The accelerometer model can be chosen by setting the =type= property:

acc_3d.type = 2; % 1: inertial mass, 2: perfect

% Number of states
% Let's first use perfect 3d accelerometers:

acc_3d.type = 2; % 1: inertial mass, 2: perfect



% And identify the dynamics from the shaker force to the measured accelerations:

%% Name of the Simulink File
mdl = 'vibration_table';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'],  1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '/acc'],  1, 'openoutput'); io_i = io_i + 1;

%% Run the linearization
Gp = linearize(mdl, io);
Gp.InputName  = {'F'};
Gp.OutputName  = {'a1', 'a2', 'a3', 'a4', 'a5', 'a6'};



% #+RESULTS:
% : size(Gp)
% : State-space model with 6 outputs, 1 inputs, and 12 states.

% We indeed have the 12 states corresponding to the 6 DoF of the suspended optical table.

% Let's now consider the inertial masses for the triaxial accelerometers:

acc_3d.type = 1; % 1: inertial mass, 2: perfect

%% Name of the Simulink File
mdl = 'vibration_table';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'],  1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '/acc'],  1, 'openoutput'); io_i = io_i + 1;

%% Run the linearization
Ga = linearize(mdl, io);
Ga.InputName  = {'F'};
Ga.OutputName  = {'a1', 'a2', 'a3', 'a4', 'a5', 'a6'};

% Resonance frequencies and mode shapes
% Let's now identify the resonance frequency and mode shapes associated with the suspension modes of the optical table.


acc_3d.type = 2; % 1: inertial mass, 2: perfect

%% Name of the Simulink File
mdl = 'vibration_table';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'],      1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '/acc_O'],  1, 'openoutput'); io_i = io_i + 1;

%% Run the linearization
G = linearize(mdl, io);
G.InputName  = {'F'};
G.OutputName  = {'ax', 'ay', 'az', 'wx', 'wy', 'wz'};



% Compute the resonance frequencies

ws = eig(G.A);
ws = ws(imag(ws) > 0);



% And the associated response of the optical table

x_mod = zeros(6, 6);

for i = 1:length(ws)
    xi = evalfr(G, ws(i));
    x_mod(:,i) = xi./norm(xi);
end

% Verify transformation

%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;

%% Name of the Simulink File
mdl = 'vibration_table';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'],  1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '/acc'],  1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/acc_O'],  1, 'openoutput'); io_i = io_i + 1;

%% Run the linearization
G = linearize(mdl, io, 0.0, options);
G.InputName  = {'F'};
G.OutputName = {'a1', 'a2', 'a3', 'a4', 'a5', 'a6', ...
                'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};

G_acc = inv(M)*G(1:6, 1);
G_id = G(7:12, 1);

bodeFig({G_acc(1), G_id(1)})
bodeFig({G_acc(2), G_id(2)})
bodeFig({G_acc(3), G_id(3)})
bodeFig({G_acc(4), G_id(4)})
bodeFig({G_acc(5), G_id(5)})
bodeFig({G_acc(6), G_id(6)})
