f_CoprFac.m

function [T11rz, T12rz, T21rz,T11dz, T12dz, T21dz]=f_CoprFac(P_ss,F,L,...
    weights)
% Youla Coprime factorization
% Works for general case of stable as well as unstable plants

[Ap, Bp, Cp, Dp] = ssdata(P_ss);

% Right coprime factorization
NumP.a=Ap-Bp*F; NumP.b=Bp; NumP.c=Cp-Dp*F; NumP.d=Dp; 
NumP=ss(NumP.a,NumP.b,NumP.c,NumP.d);
DenP.a=Ap-Bp*F; DenP.b=Bp; DenP.c=-F; DenP.d=eye(size(DenP.c,1)); 
DenP=ss(DenP.a,DenP.b,DenP.c,DenP.d);
% Controller
NumK.a=Ap-Bp*F; NumK.b=-L; NumK.c=-F; ...
    NumK.d=zeros(size(NumK.c,1),size(NumK.b,2)); 
NumK=ss(NumK.a,NumK.b,NumK.c,NumK.d);
DenK.a=Ap-Bp*F; DenK.b=L; DenK.c=Cp-Dp*F; DenK.d=eye(size(DenK.c,1)); 
DenK=ss(DenK.a,DenK.b,DenK.c,DenK.d);
% Left coprime factorization
NumPt.a=Ap-L*Cp; NumPt.b=Bp-L*Dp; NumPt.c=Cp; NumPt.d=Dp; 
NumPt=ss(NumPt.a,NumPt.b,NumPt.c,NumPt.d);
DenPt.a=Ap-L*Cp; DenPt.b=-L; DenPt.c=Cp; DenPt.d=eye(size(DenPt.c,1)); 
DenPt=ss(DenPt.a,DenPt.b,DenPt.c,DenPt.d);
% Controller
NumKt.a=Ap-L*Cp; NumKt.b=-L; NumKt.c=-F; ...
    NumKt.d=zeros(size(NumKt.c,1),size(NumKt.b,2)); 
NumKt=ss(NumKt.a,NumKt.b,NumKt.c,NumKt.d);
DenKt.a=Ap-L*Cp; DenKt.b=-(Bp-L*Dp); DenKt.c=-F; ...
    DenKt.d=eye(size(DenKt.c,1)); 
DenKt=ss(DenKt.a,DenKt.b,DenKt.c,DenKt.d);

% Feedback transfer function matrices
SOut11=DenK*DenPt; SOut12=-NumP; SOut21=DenPt;
KSOut11=NumK*DenPt; KSOut12=DenP; KSOut21=DenPt;
TOut11=NumP*NumKt; TOut12=NumP; TOut21=DenPt;
SensIn11=DenP*DenKt; SensIn12=-DenP; SensIn21=NumPt;
SInP11=NumP*DenKt; SInP12=-NumP; SInP21=NumPt;
% TIn11=NumK*NumPt; TIn12=DenP; TIn21=NumPt; % Final!!!
% TIn11=DenP*NumKt*inv(DenPt)*NumPt; TIn12=DenP; TIn21=NumPt;
TIn11=eye(size(DenP,1))-DenP*DenKt; TIn12=DenP; TIn21=NumPt;

W1 = weights.W1; 
W2 = weights.W2; 
W3 = weights.W3; 
Wd1 = weights.Wd1; 
Wd2 = weights.Wd2; 
Wd3 = weights.Wd3; 

W1c = weights.W1c; 
W2c = weights.W2c; 
W3c = weights.W3c; 
Wd1c = weights.Wd1c; 
Wd2c = weights.Wd2c; 
Wd3c = weights.Wd3c; 

if (isfield(weights,'Wni1'))
    Wni1 = weights.Wni1; 
end
if (isfield(weights,'Wni3'))
    Wni3 = weights.Wni3; 
end

% Parameterization
T11rz1=W1*SOut11; T12rz1=W1*SOut12; T21rz1=SOut21;
if isempty(W2) 
    T11rz2=ss([]); T12rz2=ss([]); T21rz2=ss([]);
else
    T11rz2=W2*KSOut11; T12rz2=W2*KSOut12; T21rz2=KSOut21;
end
if isempty(W3) 
    T11rz3=ss([]); T12rz3=ss([]); T21rz3=ss([]);
else 
    T11rz3=W3*TOut11; T12rz3=W3*TOut12; T21rz3=TOut21;
end
if isempty(Wd1)
    T11dz1=ss([]); T12dz1=ss([]); T21dz1=ss([]);
else
    T11dz1=Wd1*SensIn11; T12dz1=Wd1*SensIn12; T21dz1=SensIn21;
end
if isempty(Wd2)
    T11dz2=ss([]); T12dz2=ss([]); T21dz2=ss([]);
else
    T11dz2=Wd2*SInP11; T12dz2=Wd2*SInP12; T21dz2=SInP21;
end
if isempty(Wd3)
    T11dz3=ss([]); T12dz3=ss([]); T21dz3=ss([]);
else
    T11dz3=Wd3*TIn11; T12dz3=Wd3*TIn12; T21dz3=TIn21;
end

% Constraint tf parameterization
T11rz1c=[]; T12rz1c=[]; T21rz1c=[];
for ii=1:length(W1c)
    T11rz1c=W1c{ii}.tfm*SOut11; T12rz1c=W1c{ii}.tfm*SOut12;
    T21rz1c=SOut21;
end

% if isempty(W2c)
%     T11rz2c=[]; T12rz2c=[]; T21rz2c=[];
% else
%     T11rz2c=W2c{1}.tfm*KSOut11; 
%     T12rz2c=W2c{1}.tfm*KSOut12; T21rz2c=KSOut21;
%     T11rz2c=[T11rz2c; W2c{2}.tfm*KSOut11]; 
%     T12rz2c=[T12rz2c; W2c{2}.tfm*KSOut12];
% end
T11rz2c=[]; T12rz2c=[]; T21rz2c=[];
for ii=1:length(W2c)
    T11rz2c=[T11rz2c; W2c{ii}.tfm*KSOut11]; 
    T12rz2c=[T12rz2c; W2c{ii}.tfm*KSOut12];
end

T11rz3c=[]; T12rz3c=[]; T21rz3c=[];
for ii=1:length(W3c)
    T11rz3c=W3c{ii}.tfm*TOut11; 
    T12rz3c=W3c{ii}.tfm*TOut12; 
    T21rz3c=TOut21;
end

T11dz1c=[]; T12dz1c=[]; T21dz1c=[];
for ii=1:length(Wd1c)
    T11dz1c=Wd1c{ii}.tfm*SensIn11; 
    T12dz1c=Wd1c{ii}.tfm*SensIn12; 
    T21dz1c=SensIn21;
end

T11dz2c=[]; T12dz2c=[]; T21dz2c=[];
for ii=1:length(Wd2c)
    T11dz2c=Wd2c{ii}.tfm*SInP11; 
    T12dz2c=Wd2c{ii}.tfm*SInP12; 
    T21dz2c=SInP21;
end

T11dz3c=[]; T12dz3c=[]; T21dz3c=[];
for ii=1:length(Wd3c)
    T11dz3c=Wd3c{ii}.tfm*TIn11; 
    T12dz3c=Wd3c{ii}.tfm*TIn12; 
    T21dz3c=TIn21;
end

% For Trz1 and Tdiz2
T11rz=[T11rz1; T11rz2; T11rz3; T11rz1c; T11rz2c; T11rz3c]; 
T12rz=[T12rz1; T12rz2; T12rz3; T12rz1c; T12rz2c; T12rz3c]; 
T21rz=T21rz1;
T11dz=[T11dz1; T11dz2; T11dz3; T11dz1c; T11dz2c; T11dz3c]; 
T12dz=[T12dz1; T12dz2; T12dz3; T12dz1c; T12dz2c; T12dz3c]; 
T21dz=T21dz1;