% loop_gain_strictproper.m

% Gzw
Gzw = ss(A+B*Ft*expm(-A*h),B*Ft,Ft*expm(-A*h),Ft);
Gzw = tf(Gzw);

% Pi
P = tf(ss(A,B,eye(n),zeros(n,m)));
Pa = tf(ss(A,B,expm(-A*h),zeros(n,m)));
s = tf('s');
Delay = exp(-h*s);
Pi = ss(Pa)-P*Delay;

% Pipb
Tpb = B/2;
for ii = 1:1:(N-1)
 Delayii = ss(exp(-ii/N*h*s));
 Tpb = ss(Tpb)+expm(A*ii/N*h)*B*Delayii;
end
Tpb = ss(Tpb)+expm(A*h)*B/2*ss(Delay);
Tpb = Tpb*(h/N);
Pipb = expm(-A*h)*Tpb;

% Pidb
E = tau*(eye(n)-expm(A*h)*ss(exp(-h*s)))*B+(eye(n)+tau*A)*Tpb;
Tdb =  1/(tau*s+1)*E;
Pidb = expm(-A*h)*Tdb;

% Loop_d
Loop = Gzw*(Pidb-Pi);

% Loop_d gain
Loop_frd = frd(Loop,w,'Units','rad/s');
Loop_fdata = frdata(Loop_frd,'v');
Loop_gain = 20*log10(abs(Loop_fdata));
if m > 1
 Loop_gain = max(Loop_gain(:,1,:));
 Loop_gain = reshape(Loop_gain,1,length(Loop_gain));
end

% EOF