%% Script Description
% Make the same identification as Marc did
% Should comment out the nano-hexapod and sample before
% runing this script.

%%
clear; close all; clc;

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

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

%% Micro-Hexapod
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/Fm'],1,'input');
io(2) = linio([mdl, '/Micro-Station/Micro_Hexapod_Inertial_Sensor'],1,'output');

% Run the linearization
G_h_h_raw = linearize(mdl,io, 0);
G_h_h_raw = G_h_h_raw(1:3, 1:3);
G_h_h = preprocessIdTf(G_h_h_raw, 10, 10000);

% Input/Output names
G_h_h.InputName  = {'Fux', 'Fuy', 'Fuz'};
G_h_h.OutputName = {'Dux', 'Duy', 'Duz'};

%% Granite
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/F_granite'],1,'input');
io(2) = linio([mdl, '/Micro-Station/Granite_Inertial_Sensor'],1,'output');

% Run the linearization
G_g_g_raw = linearize(mdl,io, 0);
G_g_g = preprocessIdTf(G_g_g_raw, 10, 10000);

% Input/Output names
G_g_g.InputName  = {'Fgx', 'Fgy', 'Fgz'};
G_g_g.OutputName = {'Dgx', 'Dgy', 'Dgz'};

%% Micro Hexapod to Granite
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/Fm'],1,'input');
io(2) = linio([mdl, '/Micro-Station/Granite_Inertial_Sensor'],1,'output');

% Run the linearization
G_h_g_raw = linearize(mdl,io, 0);
G_h_g_raw = G_h_g_raw(1:3, 1:3);
G_h_g = preprocessIdTf(G_h_g_raw, 10, 10000);

% Input/Output names
G_h_g.InputName  = {'Fhx', 'Fhy', 'Fhz'};
G_h_g.OutputName = {'Dgx', 'Dgy', 'Dgz'};

%% Save the obtained transfer functions
save('./mat/id_micro_station.mat', 'G_h_h', 'G_g_g', 'G_h_g');