%Initialize
clear all
t0=clock;
m=16; %order of 12 z^{-12}


%To approximate  a IIR model represented by A B C and D
%The model is an industrila dry process rotary cement Kiln from Wes1981
A_E = [0.914 0.0800;-0.126 0.917];
B_E = [2.091 -0.0744;-0.211 -0.0156];
C_E = eye(2);
D_E = zeros(2);

A=A_E;
B=B_E; 
C=C_E; 
D=D_E;

[q,p] = size(D);
Cm=C*A^(m-1);
Am=A;
Bm=B;
Pm = dlyap(Am,Bm*Bm');
Qm = dlyap(Am',Cm'*Cm);
%Error of Direct truncation
Pmm=Pm;
Qmm=Qm;
err_dt2 = sqrt(trace(Cm*Pmm*Cm'))
err_dtsys =ltisys(Am,Bm,Cm);
[err_dtinf,peak_dtinf] = dnorminf(err_dtsys)
%invPm = inv(Pm);
E0=eig(Pm*Qm);
gama0=sqrt(max(abs(E0)))
%gama=gama0*(1+0.05);
%gama=gama0;
gama=gama0*(1+0.2); %

%Initialization of the error system, that is, e(:,1:p) is e_0=g_0-f_0.
ee=zeros(q,m*p);
 
%To compute e_{i} for i=0,...,m-1
%To compute e_{i} for i=0,...,m-1
for k=1:m
    [q1,q2]=size(Qm);
    e=-Cm*Pm*pinv(gama^2*eye(q1)-(Qm-Cm'*Cm)*Pm)*Am'*Qm*Bm;%Actually it is to compute e(m-k)
    P12=Am*Pm*Cm'+Bm*e';
    P22=Cm*Pm*Cm'+e*e';
    Pm=[Pm P12;P12' P22];
    Qm=[Qm zeros(q1,q);zeros(q,q2) eye(q)];
    Am=[Am zeros(q1,q);Cm zeros(q,q)];
    Bm=[Bm;e];
    Cm=[zeros(q,q1) eye(q)];
    ee(:,(m-k)*p+1:(m-k)*p+p) = e;
end

errhk=ltisys(Am,Bm,Cm);
[err_hkinf,peak_hkinf]= dnorminf(errhk);
err_hk2 = sqrt(trace(Cm*Pm*Cm'));
%initialization of the FIR approximation,that is, f(:,1:p) is f_0,
%f(:,p+1:2p) is f_1,...
f=zeros(q,m*p);
f(:,1:p)=D-e(:,1:p);

%To get FIR approximation model by f_{i}=g_{i}-e_{i} for i=0,...,m-1
for k=1:m-1
    g = C*A^(k-1)*B;
    %f(:,p*k+1:p*k+p) = g - ee(:,p*k+1:p*k+p);
    f(:,p*(m-k)+1:p*(m-k)+p) = g - ee(:,p*(m-k)+1:p*(m-k)+p);
    fd(:,p*(m-k)-1:p*(m-k)) = g;
end

%for k=1:m-1
 %   fff(:,p*k-p+1:p*k) = f(:,p*(m-k)-1:p*(m-k));
 %end

%To construct the system FIR approximation systems
%E_FIR computed by Hankel norm approximation is Af,Bf,Cf and Df

Af = zeros(2*(m-1),2*(m-1));
for k=1:2*(m-2)
    Af(k,k+2)=1;
end

Bf = zeros(2*(m-1),2);
Bf(2*m-3,1)=1;
Bf(2*m-2,2)=1;

Cf =f(:,p+1:m*p);
Df = f(:,1:p);

%To form the error system by Hankel norm
%firsys = ss(Af,Bf,Cf,Df);
%original = ss(A_E,B_E,C_E,D_E);
%errorsys = original-firsys;
%[Aee,Bee,Cee,Dee] = ssdata(errorsys);
% errorhk = ltisys(Aee,Bee,Cee,Dee);
% err_hkinf = dnorminf(errorhk);
t1=etime(clock,t0);

dsigma(Af,Bf,Cf,Df,1)
%%The singular value of FIR filters by Hankel norm approximation; color
%%blue
hold

%dsigma(Af,Bf,fd,D_E,1)
%%The singular value of FIR filters by direct truncation, color green


dsigma(A_E,B_E,C_E,D_E,1)%color red %The singular value plot of origin IIR system

%After running this program, to load infm12 and draw the singular value of the FIR models from hinfty method
%by the command of dsigma(A_f,B_f,C_f,D_f,1)
%The gama0 = 5.3700, gama = 6.4440=gama0*1.2.
%For Hankel norm method, err_hkinf =6.7076, peak_hkinf=-0.1092, err_hk2 = 2.945
%For direct truncation method, err_dtinf = 8.8755,peak_dtinf=-0.1092; err_dt2 = 2.417
%For hinfinity method, err_infinf =5.4293,peak_infinf = -0.1092; err_hinf2 = 5.30