%%
clear; close all; clc;

%% Load Plant
load('./mat/G_xg_to_d.mat', 'G_xg_to_d');
load('./mat/G_f_to_d.mat', 'G_1', 'G_20', 'G_50');
load('./mat/controller.mat', 'K');

%% 
S = minreal(inv(tf(eye(6))+G_20*K));
T = minreal((tf(eye(6))+G_20*K)\G_20*K);

bodeFig({S(1,1), T(1,1)})
legend({'$S_x$', '$T_x$'})

bodeFig({S(2,2), T(2,2)})
legend({'$S_y$', '$T_y$'})

bodeFig({S(3,3), T(3,3)})
legend({'$S_z$', '$T_z$'})

%%
save('./mat/T_S.mat', 'S', 'T');