% ----------------------------------------------------------------------
% Type-B1 prototype for KAGRA
% Coded by T Sekiguchi on 2015/07/29
% ----------------------------------------------------------------------
% 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('typeB1susmdl_150729.mat');  % Import ss model
typeB1proto_no_control_150729;    % Import servo filters
mdlfile='typeB1simctrl_150729';   % typeB1 ver.150729
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/20150713_TF_TM_L.txt');
bodesusplotmeascmp(sysc0,'actLTM','OSEM_LTM',freq1,data_LTM,...
    'ylim',[1e-5,1e3],'unit1','1','color',{'-r','-b','-b'},...
    'title','Transfer function from LTM actuator to LTM sensor');
export_fig('figure/typeB1proto_TFcmp2_LTM_150729.pdf')

data_PTM=importdata('measurement/20150713_TF_TM_P.txt');
bodesusplotmeascmp(sysc0,'actPTM','OSEM_PTM',freq1,data_PTM,...
    'ylim',[1e-5,1e3],'unit1','1','color',{'-r','-b','-b'},...
    'title','Transfer function from PTM actuator to PTM sensor');
export_fig('figure/typeB1proto_TFcmp2_PTM_150729.pdf')

data_YTM=importdata('measurement/20150713_TF_TM_Y.txt');
bodesusplotmeascmp(sysc0,'actYTM','OSEM_YTM',freq1,data_YTM,...
    'ylim',[1e-5,1e3],'unit1','1','color',{'-r','-b','-b'},...
    'title','Transfer function from YTM actuator to YTM sensor');
export_fig('figure/typeB1proto_TFcmp2_YTM_150729.pdf')

%% TRANSFER FUNCTION IM
data_LIM=importdata('measurement/20150713_TF_IM_L.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');
export_fig('figure/typeB1proto_TFcmp2_LIM_150729.pdf')

data_TIM=importdata('measurement/20150713_TF_IM_T.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');
export_fig('figure/typeB1proto_TFcmp2_TIM_150729.pdf')

data_VIM=importdata('measurement/20150713_TF_IM_V.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');
export_fig('figure/typeB1proto_TFcmp2_VIM_150729.pdf')

data_RIM=importdata('measurement/20150713_TF_IM_R.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');
export_fig('figure/typeB1proto_TFcmp2_RIM_150729.pdf')

data_PIM=importdata('measurement/20150713_TF_IM_P.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');
export_fig('figure/typeB1proto_TFcmp2_PIM_150729.pdf')

data_YIM=importdata('measurement/20150713_TF_IM_Y.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');
export_fig('figure/typeB1proto_TFcmp2_YIM_150729.pdf')

%% DATA