% Asimptotski estimator - sustav 3 reda
% 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

% Projekt regulatora
p1=0.7784;
p2=0.8055+0.1543j;
dcp = [p1; p2; conj(p2)]   % polovi vodjenog sustava
K = place(Fi, Psi, dcp)
disp('Pritisni bilo koju tipku za nastavak ...'), pause

% Projekt estimatora
z1=0.3672;		% polovi estimatora
z2=0.328824 + 0.310595j;
dep=[z1; z2; conj(z2)];
cest=[1 0 0]; %vektor c za proracun estimatora - dostupna je samo varijabla x1
M = place(Fi', cest', dep)'
disp('Pritisni bilo koju tipku za nastavak ...'), pause

% Formiranje ekvivalentnog regulatora
sensors = [1]; known = []; controls = [1];
Hreg = reg(Gp, K, M, sensors, known, controls);
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; 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; 0], 5);
figure(2), clf, subplot(3,1,1), plot(t,x(:,1),'b.:', t,x(:,4),'r.:'), grid
legend('x_1 = y','{x^\^}_1 = y^\^')
xlabel('vrijeme (s)'), ylabel('var.stanja x1=y')
subplot(3,1,2), plot(t,x(:,2),'b.:', t,x(:,5),'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(:,6),'r.:'), grid
legend('x_3','{x^\^}_3')
xlabel('vrijeme (s)'), ylabel('var.stanja x3')
disp('Pritisni bilo koju tipku za nastavak ...'), pause