Ts=1/5000
tf1=tf([1],[1 0])
%tfz1=tf([1],[1 0],Ts)
tfz1a=c2d(tf1,Ts);%%'zoh'=default
tfz1b=c2d(tf1,Ts,'foh');
tfz1c=c2d(tf1,Ts,'impulse');
tfz1d=c2d(tf1,Ts,'tustin');
tfz1e=c2d(tf1,Ts,'matched');
tfz1f=c2d(tf1,Ts,'least-squares');
%      'zoh'           Zero-order hold on the inputs
%       'foh'           Linear interpolation of inputs
%       'impulse'       Impulse-invariant discretization
%       'tustin'        Bilinear (Tustin) approximation.
%       'matched'       Matched pole-zero method (for SISO systems only).
%       'least-squares' Least-squares minimization of the error between
 


bo = bodeoptions;
%bo.FreqUnits = 'Hz'; bo.MagUnits='abs'; bo.Grid='on';
bo.FreqUnits = 'Hz'; bo.Grid='on';
bodeplot(tf1,tfz1a,tfz1b,tfz1c,tfz1d,tfz1e,tfz1f,bo)
bodeplot(tf1,bo)

%k=1.548e04
k=1:
tf1=tf(k ,[1 0 0]);
ss1=ss(tf1)
[Am,Bm,Cm,Dm]=ssdata(ss1);
pl=[-3 -4];
pl=[-30 -40];
pl=[-100 -110];
pl=[-1000 -1100];
%pl=[-10+40i -10-40i];
K = place(Am,Bm,pl);
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName','u','OutputName','y');
step(ss_cl);

bodeplot(ss_cl)

t=0:1E-4:.5; %time vector for simulations
u = ones(size(t));
xm0=[0 0];
[y,t,x]=lsim(ss_cl,V*u,t,xm0);


k=1.548e04
%tf1=tf(k ,[1 0 0]);
tf1=tf({[k 0]; k},[1 0 0]); %not controlabe, not observable
%tf1=tf({[k 0]; k},[1 .3 .5]); %controlabe, observable
%chkCtrlObsv(ss1,'ss1');
ss1=ss(tf1);
[Am,Bm,Cm,Dm]=ssdata(ss1);
pl=[-1500 -1600];
K = place(Am,Bm,pl);
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
V=V(end);
ss_cl = ss(Am-Bm*K,Bm*V,Cm,0); %THIS WORKS
step(ss_cl)

pb.ss_plt=ss1
pb.Kfb=K*(Cm^-1);
pb.V=V;
bode(ss_cl)


pb.ss_plt=mot{1}.ss_q;
pl=[-1000 -1100]*1;
[Am,Bm,Cm,Dm]=ssdata(pb.ss_plt);
K = place(Am,Bm,pl);
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
V=V(end);
pb.Kfb=K*(Cm^-1);
pb.V=V;

%k/s^2 -> w^2/s^2 ->frq=sqrt(k)/(w/2*pi)=sqrt(1.548e04)/(2*pi)=19.8Hz

[Am,Bm,Cm,Dm]=ssdata(mot.ss_dq);
pl=[-3300 -2100 -2500];
K = place(Am,Bm,pl);
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )

KK=K*(Cm^-1);
KK=KK(1,:);
pb.Kfb=KK
pb.V=V