%first order system
load lab3_order1_3
plot(data)
%first 30 steps
y1=data.y(31:130);
u1=data.u(31:130);
y2=data.y(110:130);
u2=data.u(110:130);

yss=mean(y2);
uss=mean(u2);
K=yss/uss;

y0=yss;
[ymax,ymax_ind]=max(y1);
yt=y0+0.368*(ymax-y0);


ind=find(y1(ymax_ind:end)<yt,1,'first');

T=t(ind);
H=tf(K,[T 1]) % THE TRANSFER FUNCTION

A=-1/T;B=K/T;C=1;D=0;
Hss = ss(A, B, C, D);

%%MSE);

l=lsim(Hss,data.u,t,y0);
plot(t,data.y,'r',t,l,'LineWidth',2);
shg
%%%%%%%%%%%---------------
%%Second order system
load lab3_order2_3
plot(data)

y1=data.y(31:130);
u1=data.u(31:130);
y2=data.y(110:130);
u2=data.u(110:130);

yss=mean(y2);
y0=yss;
uss=mean(u2);
K=yss/uss;

ind0=find(y1>yss+0.1,1,'first')-1;
t00=t(ind0);
ind1=find(y1>yss+0.1,1,'last')+1;

t01=t(ind1);
ind2=find(y1(ind1+1:end)>yss,1,'first')-1+ind1;
t02=t(ind2);


APlus=data.Ts*sum(y1(ind0:ind1)-y0);
AMin=data.Ts*sum(y0-y1(ind1:ind2));
M=AMin/APlus;
M=abs(M)


e=log(1/M)/(sqrt(pi^2+(log(M))^2));
[ymax1,ymax1_ind]=max(y1(ind0:ind1));
t1=t(31+ymax1_ind);

[ymax2,ymax2_ind]=max(y1(ind1:end));
t3=t(31+ind1+ymax2_ind);

T0=t3-t1;

wn=2*pi/(T0*sqrt(1-e^2));

H=tf(K*wn^2,[1 2*e*wn wn^2])

A=[0 1;-wn^2 -2*e*wn];
B=[0;K*wn^2];
C=[1 0];
D=0;

Hss = ss(A, B, C, D);

[yt,ty]=lsim(Hss,data.u,t,[yss;0]);
plot(t,data.y,'r',ty,yt,'LineWidth',2);
shg