% Ten skrypt ilustruje procedure projektowania 
% regulatorów minimalizuj¹cych normê Hinfty.
% Przyk³ad pokazuje ró¿ne sposoby syntezy  regulatora rozwi¹zuj¹cego 
% miesznay  problem S/KS z filtrami wagowymi (kszta³tujacymi). 

clear all;
close all;
clc;

%---------------------------------
% Definicje posystemów
%---------------------------------
%Obiekt regulacji  G=200/((10s+1)(0.05s+1)^2)

% Sposob 1, bezpoœrednio poprzez mu-tools :
G = nd2sys(1,conv([10,1],conv([0.05 1],[0.05 1])),200);
% Sposob 2, poprzez CST:
% s = tf('s');
% Gcst = 200/((10*s+1)*(0.05*s+1)^2);
% [a,b,c,d] = ssdata(balreal(Gcst));
% G = pck(a,b,c,d);


% Filtry wagowe  Ws = (s/M+w0)/(s+w0*A), Wks=1
M = 1.5; 
w0 = 10; 
A=1.e-4;
Ws = nd2sys([1/M w0],[1 w0*A]);
Wks = 1;
%---------------------------------
%Budowanie uogólnionego obiektu regulacji  P
%---------------------------------
%Sposób podstawowy:
% /z1\ /Ws -Ws*G\ /r\
% |z2| = |0 Wks | | |
% \ v/ \I -G / \u/
% Reprezentacja w dziedzinie transmitancji 
Z1 = sbs(Ws,mmult(-1,Ws,G)); % polecenie sbs s³uzy do ³¹czenia podsystemów
Z2 = sbs(0,Wks);              % mmult do mnozenia podstemów
V = sbs(1,mmult(-1,G));
P0 = abv(Z1,Z2,V);            % blokowy zapis transmitancji 

%P0 najczesciej nie jest minimalna realizacja wiec zrobmy to 
[a,b,c,d] = unpck(P0);
[ab,bb,cb,db] = ssdata( balreal( minreal( ss(a,b,c,d) ) ) );
P0 = pck(ab,bb,cb,db); % pakujemy do odpowiedniego zapisu  jeszcze raz


%---------------------------------
%Budowanie uogólnionego obiektu regulacji  P
%---------------------------------
%Sposób alternatywny:
% /z1\ /W1 -W1*G\ /r\
% |z2| = |0 W2 | | |
% \ v/ \I -G / \u/

%stanowa realizacja podsystemów:
[A,B,C,D] = unpck(G);
[A1,B1,C1,D1] = unpck(Ws);
[A2,B2,C2,D2] = unpck(Wks);
%odczytujemy liczbê wejsc i wyjsc:
n1 = size(A1,1); [q1, p1] = size(D1);
n2 = size(A2,1); [q2, p2] = size(D2);
n = size(A,1) ; [p , q ] = size(D);
%stanowa reprezentacja P:
Ap = [ A1 , zeros(n1,n2) , -B1*C ;
zeros(n2,n1) , A2 , zeros(n2,n) ;
zeros(n ,n1) , zeros(n ,n2) , A ];
Bp = [ B1 ,-B1*D;
zeros(n2,p) , B2 ;
zeros(n ,p) , B ];
Cp = [ C1 , zeros(q1,n2) , -D1*C ;
zeros(q2,n1), C2 , zeros(q2,n) ;
zeros(q ,n1), zeros(q ,n2) , -C ];
Dp = [ D1 , -D1*D;
zeros(q2,p ), D2 ;
eye(p) , -D ];
%robimy tzw. balanced realization (balreal) aby uniknac pewnych problemow
%numerycznych
[Apb,Bpb,Cpb,Dpb] = ssdata( balreal( ss(Ap,Bp,Cp,Dp) ) );
P1 = pck(Apb,Bpb,Cpb,Dpb);
%---------------------------------
%Budowanie uogólnionego obiektu regulacji  P
%---------------------------------
%Sposób aternatywny 2:

systemnames = 'G Ws Wks';
inputvar = '[r(1); u(1)]';      %wejscia sa skalarami, 
outputvar = '[Ws; Wks; r-G]';
input_to_G = '[u]';
input_to_Ws = '[r-G]';
input_to_Wks = '[u]';
sysoutname = 'P2';
cleanupsysic = 'yes';
sysic
%---------------------------------
%Budowanie uogólnionego obiektu regulacji  P
%---------------------------------
%Sposób aternatywny 3:

inputs = 'r(1); u(1)';
outputs = 'Ws; Wks; e=r-G';
K_in = []; %nie mamy jeszcze regulatora (sterownika)
G_in = 'G:u';
Ws_in = 'Ws:e';
Wks_in = 'Wks:u';

[P3,r] = sconnect(inputs,outputs,K_in,G_in,G,Ws_in,Ws,Wks_in,Wks);
%---------------------------------
%Budowanie uogólnionego obiektu regulacji  P
%---------------------------------
%Sposób aternatywny 3: korzystamy z funkcji 'iconnect':
% r = icsignal(1);
% u = icsignal(1);
% ws = icsignal(1);
% wks = icsignal(1);
% e = icsignal(1);
% y = icsignal(1);
% M = iconnect;
% M.Input = [r;u];
% M.Output = [ws;wks;e];
% M.Equation{1} = equate(e,r-y);
% M.Equation{2} = equate(y,ss(A,B,C,D)*u);
% M.Equation{3} = equate(ws,ss(A1,B1,C1,D1)*e);
% M.Equation{4} = equate(wks,ss(A2,B2,C2,D2)*u);
% [ab,bb,cb,db] = ssdata( balreal(M.System) );
% P4 = pck(ab,bb,cb,db);

%% Synteza regulatora
% Mozemy wybrac sobie dowolna metode tworzenia uogolnionego obiektu
% regulacji. Po prostu P=P0 lub P=P1 lub P=P2 lub P=P3 lub P=P4 
P = P1; %(0-4)
% i dobierz parametry optymalizacji 
nmeas = 1; nu = 1; gmn=0.5; gmx=20; tol = 0.001;

% mozemy rowniez wybrac sobie metode syntzy regulatora 
% czyli wybieramy albo "hinfsyn" lub  "hinflmi" lub "hinfric" 

%[K,CL,gopt] = hinfsyn(P,nmeas,nu,gmn,gmx,tol);
[gopt,K] = hinflmi(P,[nmeas, nu],0,tol); CL = starp(P,K,nmeas,nu);
%[gopt,K] = hinfric(P,[nmeas, nu],gmn,gmx); CL = starp(P,K,nmeas,nu);

%Jako alternastywe mo¿emy to wszystko zrobic jak ponizej 
% uzyskujac opisy stanowe wszystkich transmitancji 

%[a,b,c,d] = unpck(G); Gcst = ss(a,b,c,d);
%[a,b,c,d] = unpck(Ws); Wscst = ss(a,b,c,d);
%[a,b,c,d] = unpck(Wks); Wkscst = ss(a,b,c,d);
%[K,CL,gopt] = mixsyn(Gcst,Wscst,Wkscst,[]);
%[a,b,c,d] = ssdata( balreal(K) ); K = pck(a,b,c,d);
%[a,b,c,d] = ssdata( balreal(CL) ); CL = pck(a,b,c,d);


%% Analiza wyników 

w = logspace(-4,6,50);
CLw = vsvd(frsp(CL,w));
figure(1); vplot('liv,m',CLw);
title('singular values of weighted closed loop system');

% Tworzymy podstawowe funkcje transmitacji
[type,out,in,n] = minfo(G);
I = eye(out);
S = minv(madd(I,mmult(G,K))); %f. czulosci (transmitancja uchybowa) 
T = msub(I,S); %komplementarna f. czulosci
KS = mmult(K,S); % wyjscie regulatora (wejscie do G)
GK = mmult(G,K); % transmitncja ukladu otwartego 

% Wartoœci osobliwe jako funkcje czestotliwosci 
Sw = vsvd(frsp(S,w));
Tw = vsvd(frsp(T,w));
Kw = vsvd(frsp(K,w));
KSw = vsvd(frsp(KS,w));
GKw = vsvd(frsp(GK,w));

%Plot singular value plots
%Note: if desired, you can change vplot to plot the amplitude in dB. Type
%edit vplot and uncomment the appropriate lines in the code
figure(2); 
vplot('liv,lm',Sw,'-',Tw,'--',GKw,'-.');
title('\sigma(S(jw)) (solid), \sigma(T(jw)) (dashed) and \sigma(GK(jw)) (dashdot)');
xlabel('Frequency [rad/sec]'); 
ylabel('Amplitude')
figure(3); 
vplot('liv,lm',Kw);
title('\sigma(K(jw))');
xlabel('Frequency [rad/sec]'); 
ylabel('Amplitude')

%% Odpowiedzi czestotliwosciowe
% Sprawdzamy czy w wyniku obiczeñ 
% dostaliœmy to czego chcieliœmy

Sd = minv(Ws); 
Sdw = vsvd(frsp(Sd,w)); % po¿adana funkcja czu³oœci 
KSd = minv(Wks); 
KSdw = vsvd(frsp(KSd,w)); %po¿adane wyjscie regulatora 
figure(4); 
vplot('liv,lm',Sw,'-',Sdw,'--');
title('\sigma(S(jw)) (solid) and \sigma(Ws^{-1}(jw)) (dashed)');
xlabel('Frequency [rad/sec]'); 
ylabel('Amplitude')
figure(5); 
vplot('liv,lm',KSw,'-',KSdw,'--')
title('\sigma(KS(jw)) (solid) and \sigma(Wks^{-1}(jw)) (dashed)');
xlabel('Frequency [rad/sec]'); 
ylabel('Amplitude')

%% Odpowiedzi skokowe 
reference = 1; 
tfinal = 1; 
step = 0.01;
y = trsp(T,reference,tfinal,step);
u = trsp(KS,reference,tfinal,step);
figure(6); 
subplot(2,1,1); 
vplot('iv,d',y);
title('Step response'); 
ylabel('y');
subplot(2,1,2); 
vplot('iv,d',u);
ylabel('u'); 
xlabel('time');
