% Reducirani estimator - sustav 3 reda - mjerljive dvije varijable stanja
% 26 II 02

Fi=[1 0.0952 0.0042;0 0.9048 0.0782;0 0 0.6703];
Psi=[0.0001; 0.0042; 0.0824];
c=[1 0 0;0 1 0;0 0 1]; % kako bismo mogli nacrtati sve varijable stanja
d=[0;0;0];
T = 0.1;
Gp = ss(Fi, Psi, c, d, T);
set(Gp, 'InputName', {'u'}, 'StateName', {'x1', 'x2', 'x3'}, ...
    'OutputName', {'y1', 'y2', 'y3'})
disp('Sustav koji se vodi Gp')
zpk(Gp)
disp('Pritisni bilo koju tipku za nastavak ...'), pause

% Particija matrica
Fi_aa = Fi(1:1, 1:1);
Fi_ab = Fi(1:1, 2:3);
Fi_ba = Fi(2:3, 1:1);
Fi_bb = Fi(2:3, 2:3);

Psi_a = Psi([1]);
Psi_b = Psi([2 3]);

% Projekt regulatora
p1=0.7784;
p2=0.8055+0.1543j;
dcp = [p1; p2; conj(p2)]   % polovi vodjenog sustava
K = place(Fi, Psi, dcp)
Ka = K([1]); % povratna veza po mjerljivim varijablama
Kb = K([2 3]); % povratna veza po nemjerljivim varijablama
disp('Pritisni bilo koju tipku za nastavak ...'), pause

% Projekt estimatora
z1=0.3968+0.2j;		% polovi estimatora
dep=[z1,conj(z1)];
L = place(Fi_bb', Fi_ab', dep)'
disp('Pritisni bilo koju tipku za nastavak ...'), pause

% Formiranje ekvivalentnog regulatora
Ar = Fi_bb - L*Fi_ab - (Psi_b - L*Psi_a)*Kb;
Br = Fi_ba - L*Fi_aa - (Psi_b - L*Psi_a)*(Kb*L + Ka) + (Fi_bb - L*Fi_ab)*L;
Cr = -Kb;
Dr = -(Kb*L + Ka);   
Hreg = ss(Ar, Br, Cr, Dr, T);
disp('Regulator Hreg')
zpk(Hreg)
disp('Pritisni bilo koju tipku za nastavak ...'), pause

% Formiranje zatvorenog regulacijskog sustava
feedin = [1]; feedout = [1]; sign=+1;
Gcl = feedback(Gp, Hreg, feedin, feedout, sign);
disp('Zatvoreni regulacijski sustav Gcl')
zpk(Gcl)
disp('Pritisni bilo koju tipku za nastavak ...'), pause

% Odziv na pocetne uvjete
figure(1), clf, initial(Gcl, [0; 1; 0; 0; 0], 5), grid;
title('Odziv na pocetne uvjete x_2 = 1')
xlabel('vrijeme'), ylabel('izlaz sustava')

% Usporedba tocnosti estimacije
title('Odziv na pocetne uvjete x_2 = 1')
[y, t, x] = initial(Gcl, [0; 1; 0; 0; 0], 5);
figure(2), clf, subplot(3,1,1), plot(t,x(:,1),'b.:'), grid
legend('x_1 = y')
xlabel('vrijeme (s)'), ylabel('var.stanja x1=y')
subplot(3,1,2), plot(t,x(:,2),'b.:', t,x(:,4),'r.:'), grid
legend('x_2','{x^\^}_2')
xlabel('vrijeme (s)'), ylabel('var.stanja x2')
subplot(3,1,3), plot(t,x(:,3),'b.:', t,x(:,5),'r.:'), grid
legend('x_3','{x^\^}_3')
xlabel('vrijeme (s)'), ylabel('var.stanja x3')
disp('Pritisni bilo koju tipku za nastavak ...'), pause