load optmat1_lp.mat

%%%%%%%%%%%%%%
% ASY
%
% SB1: DARM
% SB2: DARM
% SB3: DARM
%

freq = 100;
idx = min(find(f>freq));

ASY_SB1_phi = atan( real(DARM_ASY_SB1_Q(idx) / DARM_ASY_SB1_P(idx)) );
disp(['ASY_SB1_PHASE = ', num2str(180/pi * ASY_SB1_phi), ';']);

DARM_ASY_SB1_PP =  cos(ASY_SB1_phi) * DARM_ASY_SB1_P + sin(ASY_SB1_phi) * DARM_ASY_SB1_Q;
DARM_ASY_SB1_QQ = -sin(ASY_SB1_phi) * DARM_ASY_SB1_P + cos(ASY_SB1_phi) * DARM_ASY_SB1_Q;
CARM_ASY_SB1_PP =  cos(ASY_SB1_phi) * CARM_ASY_SB1_P + sin(ASY_SB1_phi) * CARM_ASY_SB1_Q;
CARM_ASY_SB1_QQ = -sin(ASY_SB1_phi) * CARM_ASY_SB1_P + cos(ASY_SB1_phi) * CARM_ASY_SB1_Q;
MICH_ASY_SB1_PP =  cos(ASY_SB1_phi) * MICH_ASY_SB1_P + sin(ASY_SB1_phi) * MICH_ASY_SB1_Q;
MICH_ASY_SB1_QQ = -sin(ASY_SB1_phi) * MICH_ASY_SB1_P + cos(ASY_SB1_phi) * MICH_ASY_SB1_Q;
PRCL_ASY_SB1_PP =  cos(ASY_SB1_phi) * PRCL_ASY_SB1_P + sin(ASY_SB1_phi) * PRCL_ASY_SB1_Q;
PRCL_ASY_SB1_QQ = -sin(ASY_SB1_phi) * PRCL_ASY_SB1_P + cos(ASY_SB1_phi) * PRCL_ASY_SB1_Q;
SRCL_ASY_SB1_PP =  cos(ASY_SB1_phi) * SRCL_ASY_SB1_P + sin(ASY_SB1_phi) * SRCL_ASY_SB1_Q;
SRCL_ASY_SB1_QQ = -sin(ASY_SB1_phi) * SRCL_ASY_SB1_P + cos(ASY_SB1_phi) * SRCL_ASY_SB1_Q;

ASY_SB2_phi = atan( real(DARM_ASY_SB2_Q(idx) / DARM_ASY_SB2_P(idx)) );
disp(['ASY_SB2_PHASE = ', num2str(180/pi * ASY_SB2_phi), ';']);

DARM_ASY_SB2_PP =  cos(ASY_SB2_phi) * DARM_ASY_SB2_P + sin(ASY_SB2_phi) * DARM_ASY_SB2_Q;
DARM_ASY_SB2_QQ = -sin(ASY_SB2_phi) * DARM_ASY_SB2_P + cos(ASY_SB2_phi) * DARM_ASY_SB2_Q;
CARM_ASY_SB2_PP =  cos(ASY_SB2_phi) * CARM_ASY_SB2_P + sin(ASY_SB2_phi) * CARM_ASY_SB2_Q;
CARM_ASY_SB2_QQ = -sin(ASY_SB2_phi) * CARM_ASY_SB2_P + cos(ASY_SB2_phi) * CARM_ASY_SB2_Q;
MICH_ASY_SB2_PP =  cos(ASY_SB2_phi) * MICH_ASY_SB2_P + sin(ASY_SB2_phi) * MICH_ASY_SB2_Q;
MICH_ASY_SB2_QQ = -sin(ASY_SB2_phi) * MICH_ASY_SB2_P + cos(ASY_SB2_phi) * MICH_ASY_SB2_Q;
PRCL_ASY_SB2_PP =  cos(ASY_SB2_phi) * PRCL_ASY_SB2_P + sin(ASY_SB2_phi) * PRCL_ASY_SB2_Q;
PRCL_ASY_SB2_QQ = -sin(ASY_SB2_phi) * PRCL_ASY_SB2_P + cos(ASY_SB2_phi) * PRCL_ASY_SB2_Q;
SRCL_ASY_SB2_PP =  cos(ASY_SB2_phi) * SRCL_ASY_SB2_P + sin(ASY_SB2_phi) * SRCL_ASY_SB2_Q;
SRCL_ASY_SB2_QQ = -sin(ASY_SB2_phi) * SRCL_ASY_SB2_P + cos(ASY_SB2_phi) * SRCL_ASY_SB2_Q;

ASY_SB3_phi = atan( real(DARM_ASY_SB3_Q(idx) / DARM_ASY_SB3_P(idx)) );
disp(['ASY_SB3_PHASE = ', num2str(180/pi * ASY_SB3_phi), ';']);

DARM_ASY_SB3_PP =  cos(ASY_SB3_phi) * DARM_ASY_SB3_P + sin(ASY_SB3_phi) * DARM_ASY_SB3_Q;
DARM_ASY_SB3_QQ = -sin(ASY_SB3_phi) * DARM_ASY_SB3_P + cos(ASY_SB3_phi) * DARM_ASY_SB3_Q;
CARM_ASY_SB3_PP =  cos(ASY_SB3_phi) * CARM_ASY_SB3_P + sin(ASY_SB3_phi) * CARM_ASY_SB3_Q;
CARM_ASY_SB3_QQ = -sin(ASY_SB3_phi) * CARM_ASY_SB3_P + cos(ASY_SB3_phi) * CARM_ASY_SB3_Q;
MICH_ASY_SB3_PP =  cos(ASY_SB3_phi) * MICH_ASY_SB3_P + sin(ASY_SB3_phi) * MICH_ASY_SB3_Q;
MICH_ASY_SB3_QQ = -sin(ASY_SB3_phi) * MICH_ASY_SB3_P + cos(ASY_SB3_phi) * MICH_ASY_SB3_Q;
PRCL_ASY_SB3_PP =  cos(ASY_SB3_phi) * PRCL_ASY_SB3_P + sin(ASY_SB3_phi) * PRCL_ASY_SB3_Q;
PRCL_ASY_SB3_QQ = -sin(ASY_SB3_phi) * PRCL_ASY_SB3_P + cos(ASY_SB3_phi) * PRCL_ASY_SB3_Q;
SRCL_ASY_SB3_PP =  cos(ASY_SB3_phi) * SRCL_ASY_SB3_P + sin(ASY_SB3_phi) * SRCL_ASY_SB3_Q;
SRCL_ASY_SB3_QQ = -sin(ASY_SB3_phi) * SRCL_ASY_SB3_P + cos(ASY_SB3_phi) * SRCL_ASY_SB3_Q;

%%%%%%%%%%%%%%
% SYM
%
% SB1: PRCL
% SB2: PRCL
% SB3: CARM
%

freq = 100;
idx = min(find(f>freq));

SYM_SB1_phi = atan( real(PRCL_SYM_SB1_Q(idx) / PRCL_SYM_SB1_P(idx)) );
disp(['SYM_SB1_PHASE = ', num2str(180/pi * SYM_SB1_phi), ';']);

DARM_SYM_SB1_PP =  cos(SYM_SB1_phi) * DARM_SYM_SB1_P + sin(SYM_SB1_phi) * DARM_SYM_SB1_Q;
DARM_SYM_SB1_QQ = -sin(SYM_SB1_phi) * DARM_SYM_SB1_P + cos(SYM_SB1_phi) * DARM_SYM_SB1_Q;
CARM_SYM_SB1_PP =  cos(SYM_SB1_phi) * CARM_SYM_SB1_P + sin(SYM_SB1_phi) * CARM_SYM_SB1_Q;
CARM_SYM_SB1_QQ = -sin(SYM_SB1_phi) * CARM_SYM_SB1_P + cos(SYM_SB1_phi) * CARM_SYM_SB1_Q;
MICH_SYM_SB1_PP =  cos(SYM_SB1_phi) * MICH_SYM_SB1_P + sin(SYM_SB1_phi) * MICH_SYM_SB1_Q;
MICH_SYM_SB1_QQ = -sin(SYM_SB1_phi) * MICH_SYM_SB1_P + cos(SYM_SB1_phi) * MICH_SYM_SB1_Q;
PRCL_SYM_SB1_PP =  cos(SYM_SB1_phi) * PRCL_SYM_SB1_P + sin(SYM_SB1_phi) * PRCL_SYM_SB1_Q;
PRCL_SYM_SB1_QQ = -sin(SYM_SB1_phi) * PRCL_SYM_SB1_P + cos(SYM_SB1_phi) * PRCL_SYM_SB1_Q;
SRCL_SYM_SB1_PP =  cos(SYM_SB1_phi) * SRCL_SYM_SB1_P + sin(SYM_SB1_phi) * SRCL_SYM_SB1_Q;
SRCL_SYM_SB1_QQ = -sin(SYM_SB1_phi) * SRCL_SYM_SB1_P + cos(SYM_SB1_phi) * SRCL_SYM_SB1_Q;

SYM_SB2_phi = atan( real(PRCL_SYM_SB2_Q(idx) / PRCL_SYM_SB2_P(idx)) );
disp(['SYM_SB2_PHASE = ', num2str(180/pi * SYM_SB2_phi), ';']);

DARM_SYM_SB2_PP =  cos(SYM_SB2_phi) * DARM_SYM_SB2_P + sin(SYM_SB2_phi) * DARM_SYM_SB2_Q;
DARM_SYM_SB2_QQ = -sin(SYM_SB2_phi) * DARM_SYM_SB2_P + cos(SYM_SB2_phi) * DARM_SYM_SB2_Q;
CARM_SYM_SB2_PP =  cos(SYM_SB2_phi) * CARM_SYM_SB2_P + sin(SYM_SB2_phi) * CARM_SYM_SB2_Q;
CARM_SYM_SB2_QQ = -sin(SYM_SB2_phi) * CARM_SYM_SB2_P + cos(SYM_SB2_phi) * CARM_SYM_SB2_Q;
MICH_SYM_SB2_PP =  cos(SYM_SB2_phi) * MICH_SYM_SB2_P + sin(SYM_SB2_phi) * MICH_SYM_SB2_Q;
MICH_SYM_SB2_QQ = -sin(SYM_SB2_phi) * MICH_SYM_SB2_P + cos(SYM_SB2_phi) * MICH_SYM_SB2_Q;
PRCL_SYM_SB2_PP =  cos(SYM_SB2_phi) * PRCL_SYM_SB2_P + sin(SYM_SB2_phi) * PRCL_SYM_SB2_Q;
PRCL_SYM_SB2_QQ = -sin(SYM_SB2_phi) * PRCL_SYM_SB2_P + cos(SYM_SB2_phi) * PRCL_SYM_SB2_Q;
SRCL_SYM_SB2_PP =  cos(SYM_SB2_phi) * SRCL_SYM_SB2_P + sin(SYM_SB2_phi) * SRCL_SYM_SB2_Q;
SRCL_SYM_SB2_QQ = -sin(SYM_SB2_phi) * SRCL_SYM_SB2_P + cos(SYM_SB2_phi) * SRCL_SYM_SB2_Q;

SYM_SB3_phi = atan( real(CARM_SYM_SB3_Q(idx) / CARM_SYM_SB3_P(idx)) );
disp(['SYM_SB3_PHASE = ', num2str(180/pi * SYM_SB3_phi), ';']);

DARM_SYM_SB3_PP =  cos(SYM_SB3_phi) * DARM_SYM_SB3_P + sin(SYM_SB3_phi) * DARM_SYM_SB3_Q;
DARM_SYM_SB3_QQ = -sin(SYM_SB3_phi) * DARM_SYM_SB3_P + cos(SYM_SB3_phi) * DARM_SYM_SB3_Q;
CARM_SYM_SB3_PP =  cos(SYM_SB3_phi) * CARM_SYM_SB3_P + sin(SYM_SB3_phi) * CARM_SYM_SB3_Q;
CARM_SYM_SB3_QQ = -sin(SYM_SB3_phi) * CARM_SYM_SB3_P + cos(SYM_SB3_phi) * CARM_SYM_SB3_Q;
MICH_SYM_SB3_PP =  cos(SYM_SB3_phi) * MICH_SYM_SB3_P + sin(SYM_SB3_phi) * MICH_SYM_SB3_Q;
MICH_SYM_SB3_QQ = -sin(SYM_SB3_phi) * MICH_SYM_SB3_P + cos(SYM_SB3_phi) * MICH_SYM_SB3_Q;
PRCL_SYM_SB3_PP =  cos(SYM_SB3_phi) * PRCL_SYM_SB3_P + sin(SYM_SB3_phi) * PRCL_SYM_SB3_Q;
PRCL_SYM_SB3_QQ = -sin(SYM_SB3_phi) * PRCL_SYM_SB3_P + cos(SYM_SB3_phi) * PRCL_SYM_SB3_Q;
SRCL_SYM_SB3_PP =  cos(SYM_SB3_phi) * SRCL_SYM_SB3_P + sin(SYM_SB3_phi) * SRCL_SYM_SB3_Q;
SRCL_SYM_SB3_QQ = -sin(SYM_SB3_phi) * SRCL_SYM_SB3_P + cos(SYM_SB3_phi) * SRCL_SYM_SB3_Q;

%%%%%%%%%%%%%%
% POBS
%
% SB1: CARM
% SB2: SRCL
% SB3: nothing
%

freq = 100;
idx = min(find(f>freq));

POBS_SB1_phi = atan( real(CARM_POBS_SB1_Q(idx) / CARM_POBS_SB1_P(idx)) );
disp(['POBS_SB1_PHASE = ', num2str(180/pi * POBS_SB1_phi), ';']);

DARM_POBS_SB1_PP =  cos(POBS_SB1_phi) * DARM_POBS_SB1_P + sin(POBS_SB1_phi) * DARM_POBS_SB1_Q;
DARM_POBS_SB1_QQ = -sin(POBS_SB1_phi) * DARM_POBS_SB1_P + cos(POBS_SB1_phi) * DARM_POBS_SB1_Q;
CARM_POBS_SB1_PP =  cos(POBS_SB1_phi) * CARM_POBS_SB1_P + sin(POBS_SB1_phi) * CARM_POBS_SB1_Q;
CARM_POBS_SB1_QQ = -sin(POBS_SB1_phi) * CARM_POBS_SB1_P + cos(POBS_SB1_phi) * CARM_POBS_SB1_Q;
MICH_POBS_SB1_PP =  cos(POBS_SB1_phi) * MICH_POBS_SB1_P + sin(POBS_SB1_phi) * MICH_POBS_SB1_Q;
MICH_POBS_SB1_QQ = -sin(POBS_SB1_phi) * MICH_POBS_SB1_P + cos(POBS_SB1_phi) * MICH_POBS_SB1_Q;
PRCL_POBS_SB1_PP =  cos(POBS_SB1_phi) * PRCL_POBS_SB1_P + sin(POBS_SB1_phi) * PRCL_POBS_SB1_Q;
PRCL_POBS_SB1_QQ = -sin(POBS_SB1_phi) * PRCL_POBS_SB1_P + cos(POBS_SB1_phi) * PRCL_POBS_SB1_Q;
SRCL_POBS_SB1_PP =  cos(POBS_SB1_phi) * SRCL_POBS_SB1_P + sin(POBS_SB1_phi) * SRCL_POBS_SB1_Q;
SRCL_POBS_SB1_QQ = -sin(POBS_SB1_phi) * SRCL_POBS_SB1_P + cos(POBS_SB1_phi) * SRCL_POBS_SB1_Q;

POBS_SB2_phi = atan( real(SRCL_POBS_SB2_Q(idx) / SRCL_POBS_SB2_P(idx)) );
disp(['POBS_SB2_PHASE = ', num2str(180/pi * POBS_SB2_phi), ';']);

DARM_POBS_SB2_PP =  cos(POBS_SB2_phi) * DARM_POBS_SB2_P + sin(POBS_SB2_phi) * DARM_POBS_SB2_Q;
DARM_POBS_SB2_QQ = -sin(POBS_SB2_phi) * DARM_POBS_SB2_P + cos(POBS_SB2_phi) * DARM_POBS_SB2_Q;
CARM_POBS_SB2_PP =  cos(POBS_SB2_phi) * CARM_POBS_SB2_P + sin(POBS_SB2_phi) * CARM_POBS_SB2_Q;
CARM_POBS_SB2_QQ = -sin(POBS_SB2_phi) * CARM_POBS_SB2_P + cos(POBS_SB2_phi) * CARM_POBS_SB2_Q;
MICH_POBS_SB2_PP =  cos(POBS_SB2_phi) * MICH_POBS_SB2_P + sin(POBS_SB2_phi) * MICH_POBS_SB2_Q;
MICH_POBS_SB2_QQ = -sin(POBS_SB2_phi) * MICH_POBS_SB2_P + cos(POBS_SB2_phi) * MICH_POBS_SB2_Q;
PRCL_POBS_SB2_PP =  cos(POBS_SB2_phi) * PRCL_POBS_SB2_P + sin(POBS_SB2_phi) * PRCL_POBS_SB2_Q;
PRCL_POBS_SB2_QQ = -sin(POBS_SB2_phi) * PRCL_POBS_SB2_P + cos(POBS_SB2_phi) * PRCL_POBS_SB2_Q;
SRCL_POBS_SB2_PP =  cos(POBS_SB2_phi) * SRCL_POBS_SB2_P + sin(POBS_SB2_phi) * SRCL_POBS_SB2_Q;
SRCL_POBS_SB2_QQ = -sin(POBS_SB2_phi) * SRCL_POBS_SB2_P + cos(POBS_SB2_phi) * SRCL_POBS_SB2_Q;

POBS_SB3_phi = 0;
disp(['POBS_SB3_PHASE = ', num2str(180/pi * POBS_SB3_phi), ';']);

DARM_POBS_SB3_PP =  cos(POBS_SB3_phi) * DARM_POBS_SB3_P + sin(POBS_SB3_phi) * DARM_POBS_SB3_Q;
DARM_POBS_SB3_QQ = -sin(POBS_SB3_phi) * DARM_POBS_SB3_P + cos(POBS_SB3_phi) * DARM_POBS_SB3_Q;
CARM_POBS_SB3_PP =  cos(POBS_SB3_phi) * CARM_POBS_SB3_P + sin(POBS_SB3_phi) * CARM_POBS_SB3_Q;
CARM_POBS_SB3_QQ = -sin(POBS_SB3_phi) * CARM_POBS_SB3_P + cos(POBS_SB3_phi) * CARM_POBS_SB3_Q;
MICH_POBS_SB3_PP =  cos(POBS_SB3_phi) * MICH_POBS_SB3_P + sin(POBS_SB3_phi) * MICH_POBS_SB3_Q;
MICH_POBS_SB3_QQ = -sin(POBS_SB3_phi) * MICH_POBS_SB3_P + cos(POBS_SB3_phi) * MICH_POBS_SB3_Q;
PRCL_POBS_SB3_PP =  cos(POBS_SB3_phi) * PRCL_POBS_SB3_P + sin(POBS_SB3_phi) * PRCL_POBS_SB3_Q;
PRCL_POBS_SB3_QQ = -sin(POBS_SB3_phi) * PRCL_POBS_SB3_P + cos(POBS_SB3_phi) * PRCL_POBS_SB3_Q;
SRCL_POBS_SB3_PP =  cos(POBS_SB3_phi) * SRCL_POBS_SB3_P + sin(POBS_SB3_phi) * SRCL_POBS_SB3_Q;
SRCL_POBS_SB3_QQ = -sin(POBS_SB3_phi) * SRCL_POBS_SB3_P + cos(POBS_SB3_phi) * SRCL_POBS_SB3_Q;

%%%%%%%%%%%%%%
% XP
%
% SB1: DARM
% SB2: DARM
% SB3: nothing
%

freq = 100;
idx = min(find(f>freq));

XP_SB1_phi = atan( real(DARM_XP_SB1_Q(idx) / DARM_XP_SB1_P(idx)) );
disp(['XP_SB1_PHASE = ', num2str(180/pi * XP_SB1_phi), ';']);

DARM_XP_SB1_PP =  cos(XP_SB1_phi) * DARM_XP_SB1_P + sin(XP_SB1_phi) * DARM_XP_SB1_Q;
DARM_XP_SB1_QQ = -sin(XP_SB1_phi) * DARM_XP_SB1_P + cos(XP_SB1_phi) * DARM_XP_SB1_Q;
CARM_XP_SB1_PP =  cos(XP_SB1_phi) * CARM_XP_SB1_P + sin(XP_SB1_phi) * CARM_XP_SB1_Q;
CARM_XP_SB1_QQ = -sin(XP_SB1_phi) * CARM_XP_SB1_P + cos(XP_SB1_phi) * CARM_XP_SB1_Q;
MICH_XP_SB1_PP =  cos(XP_SB1_phi) * MICH_XP_SB1_P + sin(XP_SB1_phi) * MICH_XP_SB1_Q;
MICH_XP_SB1_QQ = -sin(XP_SB1_phi) * MICH_XP_SB1_P + cos(XP_SB1_phi) * MICH_XP_SB1_Q;
PRCL_XP_SB1_PP =  cos(XP_SB1_phi) * PRCL_XP_SB1_P + sin(XP_SB1_phi) * PRCL_XP_SB1_Q;
PRCL_XP_SB1_QQ = -sin(XP_SB1_phi) * PRCL_XP_SB1_P + cos(XP_SB1_phi) * PRCL_XP_SB1_Q;
SRCL_XP_SB1_PP =  cos(XP_SB1_phi) * SRCL_XP_SB1_P + sin(XP_SB1_phi) * SRCL_XP_SB1_Q;
SRCL_XP_SB1_QQ = -sin(XP_SB1_phi) * SRCL_XP_SB1_P + cos(XP_SB1_phi) * SRCL_XP_SB1_Q;

XP_SB2_phi = atan( real(DARM_XP_SB2_Q(idx) / DARM_XP_SB2_P(idx)) );
disp(['XP_SB2_PHASE = ', num2str(180/pi * XP_SB2_phi), ';']);

DARM_XP_SB2_PP =  cos(XP_SB2_phi) * DARM_XP_SB2_P + sin(XP_SB2_phi) * DARM_XP_SB2_Q;
DARM_XP_SB2_QQ = -sin(XP_SB2_phi) * DARM_XP_SB2_P + cos(XP_SB2_phi) * DARM_XP_SB2_Q;
CARM_XP_SB2_PP =  cos(XP_SB2_phi) * CARM_XP_SB2_P + sin(XP_SB2_phi) * CARM_XP_SB2_Q;
CARM_XP_SB2_QQ = -sin(XP_SB2_phi) * CARM_XP_SB2_P + cos(XP_SB2_phi) * CARM_XP_SB2_Q;
MICH_XP_SB2_PP =  cos(XP_SB2_phi) * MICH_XP_SB2_P + sin(XP_SB2_phi) * MICH_XP_SB2_Q;
MICH_XP_SB2_QQ = -sin(XP_SB2_phi) * MICH_XP_SB2_P + cos(XP_SB2_phi) * MICH_XP_SB2_Q;
PRCL_XP_SB2_PP =  cos(XP_SB2_phi) * PRCL_XP_SB2_P + sin(XP_SB2_phi) * PRCL_XP_SB2_Q;
PRCL_XP_SB2_QQ = -sin(XP_SB2_phi) * PRCL_XP_SB2_P + cos(XP_SB2_phi) * PRCL_XP_SB2_Q;
SRCL_XP_SB2_PP =  cos(XP_SB2_phi) * SRCL_XP_SB2_P + sin(XP_SB2_phi) * SRCL_XP_SB2_Q;
SRCL_XP_SB2_QQ = -sin(XP_SB2_phi) * SRCL_XP_SB2_P + cos(XP_SB2_phi) * SRCL_XP_SB2_Q;

XP_SB3_phi = 0;
disp(['XP_SB3_PHASE = ', num2str(180/pi * XP_SB3_phi), ';']);

DARM_XP_SB3_PP =  cos(XP_SB3_phi) * DARM_XP_SB3_P + sin(XP_SB3_phi) * DARM_XP_SB3_Q;
DARM_XP_SB3_QQ = -sin(XP_SB3_phi) * DARM_XP_SB3_P + cos(XP_SB3_phi) * DARM_XP_SB3_Q;
CARM_XP_SB3_PP =  cos(XP_SB3_phi) * CARM_XP_SB3_P + sin(XP_SB3_phi) * CARM_XP_SB3_Q;
CARM_XP_SB3_QQ = -sin(XP_SB3_phi) * CARM_XP_SB3_P + cos(XP_SB3_phi) * CARM_XP_SB3_Q;
MICH_XP_SB3_PP =  cos(XP_SB3_phi) * MICH_XP_SB3_P + sin(XP_SB3_phi) * MICH_XP_SB3_Q;
MICH_XP_SB3_QQ = -sin(XP_SB3_phi) * MICH_XP_SB3_P + cos(XP_SB3_phi) * MICH_XP_SB3_Q;
PRCL_XP_SB3_PP =  cos(XP_SB3_phi) * PRCL_XP_SB3_P + sin(XP_SB3_phi) * PRCL_XP_SB3_Q;
PRCL_XP_SB3_QQ = -sin(XP_SB3_phi) * PRCL_XP_SB3_P + cos(XP_SB3_phi) * PRCL_XP_SB3_Q;
SRCL_XP_SB3_PP =  cos(XP_SB3_phi) * SRCL_XP_SB3_P + sin(XP_SB3_phi) * SRCL_XP_SB3_Q;
SRCL_XP_SB3_QQ = -sin(XP_SB3_phi) * SRCL_XP_SB3_P + cos(XP_SB3_phi) * SRCL_XP_SB3_Q;

%%%%%%%%%%%%%%
% YP
%
% SB1: DARM
% SB2: DARM
% SB3: nothing
%

freq = 100;
idx = min(find(f>freq));

YP_SB1_phi = atan( real(DARM_YP_SB1_Q(idx) / DARM_YP_SB1_P(idx)) );
disp(['YP_SB1_PHASE = ', num2str(180/pi * YP_SB1_phi), ';']);

DARM_YP_SB1_PP =  cos(YP_SB1_phi) * DARM_YP_SB1_P + sin(YP_SB1_phi) * DARM_YP_SB1_Q;
DARM_YP_SB1_QQ = -sin(YP_SB1_phi) * DARM_YP_SB1_P + cos(YP_SB1_phi) * DARM_YP_SB1_Q;
CARM_YP_SB1_PP =  cos(YP_SB1_phi) * CARM_YP_SB1_P + sin(YP_SB1_phi) * CARM_YP_SB1_Q;
CARM_YP_SB1_QQ = -sin(YP_SB1_phi) * CARM_YP_SB1_P + cos(YP_SB1_phi) * CARM_YP_SB1_Q;
MICH_YP_SB1_PP =  cos(YP_SB1_phi) * MICH_YP_SB1_P + sin(YP_SB1_phi) * MICH_YP_SB1_Q;
MICH_YP_SB1_QQ = -sin(YP_SB1_phi) * MICH_YP_SB1_P + cos(YP_SB1_phi) * MICH_YP_SB1_Q;
PRCL_YP_SB1_PP =  cos(YP_SB1_phi) * PRCL_YP_SB1_P + sin(YP_SB1_phi) * PRCL_YP_SB1_Q;
PRCL_YP_SB1_QQ = -sin(YP_SB1_phi) * PRCL_YP_SB1_P + cos(YP_SB1_phi) * PRCL_YP_SB1_Q;
SRCL_YP_SB1_PP =  cos(YP_SB1_phi) * SRCL_YP_SB1_P + sin(YP_SB1_phi) * SRCL_YP_SB1_Q;
SRCL_YP_SB1_QQ = -sin(YP_SB1_phi) * SRCL_YP_SB1_P + cos(YP_SB1_phi) * SRCL_YP_SB1_Q;

YP_SB2_phi = atan( real(DARM_YP_SB2_Q(idx) / DARM_YP_SB2_P(idx)) );
disp(['YP_SB2_PHASE = ', num2str(180/pi * YP_SB2_phi), ';']);

DARM_YP_SB2_PP =  cos(YP_SB2_phi) * DARM_YP_SB2_P + sin(YP_SB2_phi) * DARM_YP_SB2_Q;
DARM_YP_SB2_QQ = -sin(YP_SB2_phi) * DARM_YP_SB2_P + cos(YP_SB2_phi) * DARM_YP_SB2_Q;
CARM_YP_SB2_PP =  cos(YP_SB2_phi) * CARM_YP_SB2_P + sin(YP_SB2_phi) * CARM_YP_SB2_Q;
CARM_YP_SB2_QQ = -sin(YP_SB2_phi) * CARM_YP_SB2_P + cos(YP_SB2_phi) * CARM_YP_SB2_Q;
MICH_YP_SB2_PP =  cos(YP_SB2_phi) * MICH_YP_SB2_P + sin(YP_SB2_phi) * MICH_YP_SB2_Q;
MICH_YP_SB2_QQ = -sin(YP_SB2_phi) * MICH_YP_SB2_P + cos(YP_SB2_phi) * MICH_YP_SB2_Q;
PRCL_YP_SB2_PP =  cos(YP_SB2_phi) * PRCL_YP_SB2_P + sin(YP_SB2_phi) * PRCL_YP_SB2_Q;
PRCL_YP_SB2_QQ = -sin(YP_SB2_phi) * PRCL_YP_SB2_P + cos(YP_SB2_phi) * PRCL_YP_SB2_Q;
SRCL_YP_SB2_PP =  cos(YP_SB2_phi) * SRCL_YP_SB2_P + sin(YP_SB2_phi) * SRCL_YP_SB2_Q;
SRCL_YP_SB2_QQ = -sin(YP_SB2_phi) * SRCL_YP_SB2_P + cos(YP_SB2_phi) * SRCL_YP_SB2_Q;

YP_SB3_phi = 0;
disp(['YP_SB3_PHASE = ', num2str(180/pi * YP_SB3_phi), ';']);

DARM_YP_SB3_PP =  cos(YP_SB3_phi) * DARM_YP_SB3_P + sin(YP_SB3_phi) * DARM_YP_SB3_Q;
DARM_YP_SB3_QQ = -sin(YP_SB3_phi) * DARM_YP_SB3_P + cos(YP_SB3_phi) * DARM_YP_SB3_Q;
CARM_YP_SB3_PP =  cos(YP_SB3_phi) * CARM_YP_SB3_P + sin(YP_SB3_phi) * CARM_YP_SB3_Q;
CARM_YP_SB3_QQ = -sin(YP_SB3_phi) * CARM_YP_SB3_P + cos(YP_SB3_phi) * CARM_YP_SB3_Q;
MICH_YP_SB3_PP =  cos(YP_SB3_phi) * MICH_YP_SB3_P + sin(YP_SB3_phi) * MICH_YP_SB3_Q;
MICH_YP_SB3_QQ = -sin(YP_SB3_phi) * MICH_YP_SB3_P + cos(YP_SB3_phi) * MICH_YP_SB3_Q;
PRCL_YP_SB3_PP =  cos(YP_SB3_phi) * PRCL_YP_SB3_P + sin(YP_SB3_phi) * PRCL_YP_SB3_Q;
PRCL_YP_SB3_QQ = -sin(YP_SB3_phi) * PRCL_YP_SB3_P + cos(YP_SB3_phi) * PRCL_YP_SB3_Q;
SRCL_YP_SB3_PP =  cos(YP_SB3_phi) * SRCL_YP_SB3_P + sin(YP_SB3_phi) * SRCL_YP_SB3_Q;
SRCL_YP_SB3_QQ = -sin(YP_SB3_phi) * SRCL_YP_SB3_P + cos(YP_SB3_phi) * SRCL_YP_SB3_Q;

%%%%%%%%%%%%%%
% POX
%
% SB1: CARM
% SB2: SRCL
% SB3: CARM
%

freq = 100;
idx = min(find(f>freq));

POX_SB1_phi = atan( real(CARM_POX_SB1_Q(idx) / CARM_POX_SB1_P(idx)) );
disp(['POX_SB1_PHASE = ', num2str(180/pi * POX_SB1_phi), ';']);

DARM_POX_SB1_PP =  cos(POX_SB1_phi) * DARM_POX_SB1_P + sin(POX_SB1_phi) * DARM_POX_SB1_Q;
DARM_POX_SB1_QQ = -sin(POX_SB1_phi) * DARM_POX_SB1_P + cos(POX_SB1_phi) * DARM_POX_SB1_Q;
CARM_POX_SB1_PP =  cos(POX_SB1_phi) * CARM_POX_SB1_P + sin(POX_SB1_phi) * CARM_POX_SB1_Q;
CARM_POX_SB1_QQ = -sin(POX_SB1_phi) * CARM_POX_SB1_P + cos(POX_SB1_phi) * CARM_POX_SB1_Q;
MICH_POX_SB1_PP =  cos(POX_SB1_phi) * MICH_POX_SB1_P + sin(POX_SB1_phi) * MICH_POX_SB1_Q;
MICH_POX_SB1_QQ = -sin(POX_SB1_phi) * MICH_POX_SB1_P + cos(POX_SB1_phi) * MICH_POX_SB1_Q;
PRCL_POX_SB1_PP =  cos(POX_SB1_phi) * PRCL_POX_SB1_P + sin(POX_SB1_phi) * PRCL_POX_SB1_Q;
PRCL_POX_SB1_QQ = -sin(POX_SB1_phi) * PRCL_POX_SB1_P + cos(POX_SB1_phi) * PRCL_POX_SB1_Q;
SRCL_POX_SB1_PP =  cos(POX_SB1_phi) * SRCL_POX_SB1_P + sin(POX_SB1_phi) * SRCL_POX_SB1_Q;
SRCL_POX_SB1_QQ = -sin(POX_SB1_phi) * SRCL_POX_SB1_P + cos(POX_SB1_phi) * SRCL_POX_SB1_Q;

POX_SB2_phi = atan( real(SRCL_POX_SB2_Q(idx) / SRCL_POX_SB2_P(idx)) );
disp(['POX_SB2_PHASE = ', num2str(180/pi * POX_SB2_phi), ';']);

DARM_POX_SB2_PP =  cos(POX_SB2_phi) * DARM_POX_SB2_P + sin(POX_SB2_phi) * DARM_POX_SB2_Q;
DARM_POX_SB2_QQ = -sin(POX_SB2_phi) * DARM_POX_SB2_P + cos(POX_SB2_phi) * DARM_POX_SB2_Q;
CARM_POX_SB2_PP =  cos(POX_SB2_phi) * CARM_POX_SB2_P + sin(POX_SB2_phi) * CARM_POX_SB2_Q;
CARM_POX_SB2_QQ = -sin(POX_SB2_phi) * CARM_POX_SB2_P + cos(POX_SB2_phi) * CARM_POX_SB2_Q;
MICH_POX_SB2_PP =  cos(POX_SB2_phi) * MICH_POX_SB2_P + sin(POX_SB2_phi) * MICH_POX_SB2_Q;
MICH_POX_SB2_QQ = -sin(POX_SB2_phi) * MICH_POX_SB2_P + cos(POX_SB2_phi) * MICH_POX_SB2_Q;
PRCL_POX_SB2_PP =  cos(POX_SB2_phi) * PRCL_POX_SB2_P + sin(POX_SB2_phi) * PRCL_POX_SB2_Q;
PRCL_POX_SB2_QQ = -sin(POX_SB2_phi) * PRCL_POX_SB2_P + cos(POX_SB2_phi) * PRCL_POX_SB2_Q;
SRCL_POX_SB2_PP =  cos(POX_SB2_phi) * SRCL_POX_SB2_P + sin(POX_SB2_phi) * SRCL_POX_SB2_Q;
SRCL_POX_SB2_QQ = -sin(POX_SB2_phi) * SRCL_POX_SB2_P + cos(POX_SB2_phi) * SRCL_POX_SB2_Q;

POX_SB3_phi = atan( real(CARM_POX_SB3_Q(idx) / CARM_POX_SB3_P(idx)) );
disp(['POX_SB3_PHASE = ', num2str(180/pi * POX_SB3_phi), ';']);

DARM_POX_SB3_PP =  cos(POX_SB3_phi) * DARM_POX_SB3_P + sin(POX_SB3_phi) * DARM_POX_SB3_Q;
DARM_POX_SB3_QQ = -sin(POX_SB3_phi) * DARM_POX_SB3_P + cos(POX_SB3_phi) * DARM_POX_SB3_Q;
CARM_POX_SB3_PP =  cos(POX_SB3_phi) * CARM_POX_SB3_P + sin(POX_SB3_phi) * CARM_POX_SB3_Q;
CARM_POX_SB3_QQ = -sin(POX_SB3_phi) * CARM_POX_SB3_P + cos(POX_SB3_phi) * CARM_POX_SB3_Q;
MICH_POX_SB3_PP =  cos(POX_SB3_phi) * MICH_POX_SB3_P + sin(POX_SB3_phi) * MICH_POX_SB3_Q;
MICH_POX_SB3_QQ = -sin(POX_SB3_phi) * MICH_POX_SB3_P + cos(POX_SB3_phi) * MICH_POX_SB3_Q;
PRCL_POX_SB3_PP =  cos(POX_SB3_phi) * PRCL_POX_SB3_P + sin(POX_SB3_phi) * PRCL_POX_SB3_Q;
PRCL_POX_SB3_QQ = -sin(POX_SB3_phi) * PRCL_POX_SB3_P + cos(POX_SB3_phi) * PRCL_POX_SB3_Q;
SRCL_POX_SB3_PP =  cos(POX_SB3_phi) * SRCL_POX_SB3_P + sin(POX_SB3_phi) * SRCL_POX_SB3_Q;
SRCL_POX_SB3_QQ = -sin(POX_SB3_phi) * SRCL_POX_SB3_P + cos(POX_SB3_phi) * SRCL_POX_SB3_Q;

%%%%%%%%%%%%%%
% POY
%
% SB1: CARM
% SB2: SRCL
% SB3: CARM
%

freq = 100;
idx = min(find(f>freq));

POY_SB1_phi = atan( real(CARM_POY_SB1_Q(idx) / CARM_POY_SB1_P(idx)) );
disp(['POY_SB1_PHASE = ', num2str(180/pi * POY_SB1_phi), ';']);

DARM_POY_SB1_PP =  cos(POY_SB1_phi) * DARM_POY_SB1_P + sin(POY_SB1_phi) * DARM_POY_SB1_Q;
DARM_POY_SB1_QQ = -sin(POY_SB1_phi) * DARM_POY_SB1_P + cos(POY_SB1_phi) * DARM_POY_SB1_Q;
CARM_POY_SB1_PP =  cos(POY_SB1_phi) * CARM_POY_SB1_P + sin(POY_SB1_phi) * CARM_POY_SB1_Q;
CARM_POY_SB1_QQ = -sin(POY_SB1_phi) * CARM_POY_SB1_P + cos(POY_SB1_phi) * CARM_POY_SB1_Q;
MICH_POY_SB1_PP =  cos(POY_SB1_phi) * MICH_POY_SB1_P + sin(POY_SB1_phi) * MICH_POY_SB1_Q;
MICH_POY_SB1_QQ = -sin(POY_SB1_phi) * MICH_POY_SB1_P + cos(POY_SB1_phi) * MICH_POY_SB1_Q;
PRCL_POY_SB1_PP =  cos(POY_SB1_phi) * PRCL_POY_SB1_P + sin(POY_SB1_phi) * PRCL_POY_SB1_Q;
PRCL_POY_SB1_QQ = -sin(POY_SB1_phi) * PRCL_POY_SB1_P + cos(POY_SB1_phi) * PRCL_POY_SB1_Q;
SRCL_POY_SB1_PP =  cos(POY_SB1_phi) * SRCL_POY_SB1_P + sin(POY_SB1_phi) * SRCL_POY_SB1_Q;
SRCL_POY_SB1_QQ = -sin(POY_SB1_phi) * SRCL_POY_SB1_P + cos(POY_SB1_phi) * SRCL_POY_SB1_Q;

POY_SB2_phi = atan( real(SRCL_POY_SB2_Q(idx) / SRCL_POY_SB2_P(idx)) );
disp(['POY_SB2_PHASE = ', num2str(180/pi * POY_SB2_phi), ';']);

DARM_POY_SB2_PP =  cos(POY_SB2_phi) * DARM_POY_SB2_P + sin(POY_SB2_phi) * DARM_POY_SB2_Q;
DARM_POY_SB2_QQ = -sin(POY_SB2_phi) * DARM_POY_SB2_P + cos(POY_SB2_phi) * DARM_POY_SB2_Q;
CARM_POY_SB2_PP =  cos(POY_SB2_phi) * CARM_POY_SB2_P + sin(POY_SB2_phi) * CARM_POY_SB2_Q;
CARM_POY_SB2_QQ = -sin(POY_SB2_phi) * CARM_POY_SB2_P + cos(POY_SB2_phi) * CARM_POY_SB2_Q;
MICH_POY_SB2_PP =  cos(POY_SB2_phi) * MICH_POY_SB2_P + sin(POY_SB2_phi) * MICH_POY_SB2_Q;
MICH_POY_SB2_QQ = -sin(POY_SB2_phi) * MICH_POY_SB2_P + cos(POY_SB2_phi) * MICH_POY_SB2_Q;
PRCL_POY_SB2_PP =  cos(POY_SB2_phi) * PRCL_POY_SB2_P + sin(POY_SB2_phi) * PRCL_POY_SB2_Q;
PRCL_POY_SB2_QQ = -sin(POY_SB2_phi) * PRCL_POY_SB2_P + cos(POY_SB2_phi) * PRCL_POY_SB2_Q;
SRCL_POY_SB2_PP =  cos(POY_SB2_phi) * SRCL_POY_SB2_P + sin(POY_SB2_phi) * SRCL_POY_SB2_Q;
SRCL_POY_SB2_QQ = -sin(POY_SB2_phi) * SRCL_POY_SB2_P + cos(POY_SB2_phi) * SRCL_POY_SB2_Q;

POY_SB3_phi = atan( real(CARM_POY_SB3_Q(idx) / CARM_POY_SB3_P(idx)) );
disp(['POY_SB3_PHASE = ', num2str(180/pi * POY_SB3_phi), ';']);

DARM_POY_SB3_PP =  cos(POY_SB3_phi) * DARM_POY_SB3_P + sin(POY_SB3_phi) * DARM_POY_SB3_Q;
DARM_POY_SB3_QQ = -sin(POY_SB3_phi) * DARM_POY_SB3_P + cos(POY_SB3_phi) * DARM_POY_SB3_Q;
CARM_POY_SB3_PP =  cos(POY_SB3_phi) * CARM_POY_SB3_P + sin(POY_SB3_phi) * CARM_POY_SB3_Q;
CARM_POY_SB3_QQ = -sin(POY_SB3_phi) * CARM_POY_SB3_P + cos(POY_SB3_phi) * CARM_POY_SB3_Q;
MICH_POY_SB3_PP =  cos(POY_SB3_phi) * MICH_POY_SB3_P + sin(POY_SB3_phi) * MICH_POY_SB3_Q;
MICH_POY_SB3_QQ = -sin(POY_SB3_phi) * MICH_POY_SB3_P + cos(POY_SB3_phi) * MICH_POY_SB3_Q;
PRCL_POY_SB3_PP =  cos(POY_SB3_phi) * PRCL_POY_SB3_P + sin(POY_SB3_phi) * PRCL_POY_SB3_Q;
PRCL_POY_SB3_QQ = -sin(POY_SB3_phi) * PRCL_POY_SB3_P + cos(POY_SB3_phi) * PRCL_POY_SB3_Q;
SRCL_POY_SB3_PP =  cos(POY_SB3_phi) * SRCL_POY_SB3_P + sin(POY_SB3_phi) * SRCL_POY_SB3_Q;
SRCL_POY_SB3_QQ = -sin(POY_SB3_phi) * SRCL_POY_SB3_P + cos(POY_SB3_phi) * SRCL_POY_SB3_Q;


% save results
save optmat1_lp_tuned.mat f *_PP *_QQ *_phi
