% Asimptotski estimator - sustav 2 reda - dvostruki integrator
% 26 II 02

Fi=[1 0.1;0 1];
Psi=[0.005;0.1];
c=[1 0];
d=[0];
T = 0.01;
Gp = ss(Fi, Psi, c, d, T);
set(Gp, 'InputName', {'u'}, 'StateName', {'x1', 'x2'}, ...
    'OutputName', 'y')
disp('Sustav koji se vodi Gp')
zpk(Gp)
zpk(Gp)
disp('Pritisni bilo koju tipku za nastavak ...'), pause

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

% Projekt estimatora
z1=0;    % polovi estimatora
z2=0.001;
dep=[z1; z2];
M = place(Fi', c', 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,subplot(3,1,1), initial(Gcl, [0.1; 0; 0; 0], 0.5), grid;
title('Odziv na pocetne uvjete x_1 = 0.1')
xlabel('vrijeme'), ylabel('izlaz sustava')

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