% ----------------------------------------------------------------------
% Type-Bp prototype for KAGRA
% Coded by A Shoda on 2017/07/04
% ----------------------------------------------------------------------
% This codes calculate force-displacement transfer functions and compare
% them with measurement results.
% ----------------------------------------------------------------------


%% PRELIMINARY
clear all;                     % Clear workspace
close all;                     % Close plot windows
addpath('../../../../utility');      % Add path to utilities
addpath('servofilter');        % Add path to servo
g = 9.81;                      % Gravity constant

%% MODEL IMPORT
load('PR3sus_170704mdl.mat');  % Import ss model
typeBp170704_no_control;    % Import servo filters
mdlfile='typeBpsimctrl_170704';   % typeBp ver.170704
st     =linmod(mdlfile);          % Linearize simulink model
invl   =strrep(st.InputName, [mdlfile,'/'],'');
outvl  =strrep(st.OutputName,[mdlfile,'/'],'');
sysc0  =ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

%% FREQUENCY SET
freq1=logspace(-2,2.5,1001);

%% TRANSFER FUNCTION TM
data_LTM=importdata('measurement/TF_TML.txt');
bodesusplotmeascmp(sysc0,'actLTM','OpLev_LTM',freq1,data_LTM,...
    'ylim',[1e-5,1e3],'unit1','1','color',{'-r','-b','-b'},...
    'title','Transfer function from LTM actuator to LTM sensor');
saveas(gcf, 'fig/TFcmp_LTM_170531', 'pdf')

data_PTM=importdata('measurement/TF_TMP.txt');
bodesusplotmeascmp(sysc0,'actPTM','OpLev_PTM',freq1,data_PTM,...
    'ylim',[1e-5,1e3],'unit1','1','color',{'-r','-b','-b'},...
    'title','Transfer function from PTM actuator to PTM sensor');
saveas(gcf, 'fig/TFcmp_PTM_170531', 'pdf')

data_YTM=importdata('measurement/TF_TMY.txt');
bodesusplotmeascmp(sysc0,'actYTM','OpLev_YTM',freq1,data_YTM,...
    'ylim',[1e-5,1e3],'unit1','1','color',{'-r','-b','-b'},...
    'title','Transfer function from YTM actuator to YTM sensor');
saveas(gcf, 'fig/TFcmp_YTM_170531', 'pdf')

%% TRANSFER FUNCTION IM
data_LIM=importdata('measurement/TF_IML.txt');


bodesusplotmeascmp(sysc0,'actLIM','OSEM_LIM',freq1,data_LIM,...
    'ylim',[1e-5,1e3],'unit1','1','color',{'-r','-b','-b'},...
    'title','Transfer function from LIM actuator to LIM sensor');
saveas(gcf, 'fig/TFcmp_LIM_170531', 'pdf')

data_TIM=importdata('measurement/TF_IMT.txt');
bodesusplotmeascmp(sysc0,'actTIM','OSEM_TIM',freq1,data_TIM,...
    'ylim',[1e-5,1e3],'unit1','1','color',{'-r','-b','-b'},...
    'title','Transfer function from TIM actuator to TIM sensor');
saveas(gcf, 'fig/TFcmp_TIM_170531', 'pdf')

data_VIM=importdata('measurement/TF_IMV.txt');
bodesusplotmeascmp(sysc0,'actVIM','OSEM_VIM',freq1,data_VIM,...
    'ylim',[1e-5,1e3],'unit1','1','color',{'-r','-b','-b'},...
    'title','Transfer function from VIM actuator to VIM sensor');
saveas(gcf, 'fig/TFcmp_VIM_170531', 'pdf')

data_RIM=importdata('measurement/TF_IMR.txt');
bodesusplotmeascmp(sysc0,'actRIM','OSEM_RIM',freq1,data_RIM,...
    'ylim',[1e-5,1e3],'unit1','1','color',{'-r','-b','-b'},...
    'title','Transfer function from RIM actuator to RIM sensor');
saveas(gcf, 'fig/TFcmp_RIM_170531', 'pdf')

data_PIM=importdata('measurement/TF_IMP.txt');
bodesusplotmeascmp(sysc0,'actPIM','OSEM_PIM',freq1,data_PIM,...
    'ylim',[1e-5,1e3],'unit1','1','color',{'-r','-b','-b'},...
    'title','Transfer function from PIM actuator to PIM sensor');
saveas(gcf, 'fig/TFcmp_PIM_170531', 'pdf')

data_YIM=importdata('measurement/TF_IMY.txt');
bodesusplotmeascmp(sysc0,'actYIM','OSEM_YIM',freq1,data_YIM,...
    'ylim',[1e-6,1e2],'unit1','1','color',{'-r','-b','-b'},...
    'title','Transfer function from YIM actuator to YIM sensor');
saveas(gcf, 'fig/TFcmp_YIM_170531', 'pdf')

%% DATA