w1=2*pi;
w2=23*pi;
XS=0.025;
mu=0.1;
tau=1/60;
a=1;
sig=0.0056;
V=10;
A=[0 1 0 0;-w1^2 -2*XS*w1 w1^2 2*XS*w1;0 0 0 1; w1^2/mu 2*XS*w1/mu -w1^2/mu-w2^2 -2*XS*w1/mu]
B=[0 0;1 0;0 0;-1/mu w2^2]
C=[1 0 -1 0;0 0 1 0;-w1^2 -2*XS*w1 w1^2 2*XS*w1]
D=[0 0;0 -1;1 0]
sys1=ss(A,B,C,D);
sysr=tf(sig*2*a*V,[1 a*V]);
sysd=tf(1,[tau 1]);
sysrd=append(sysd,sysr)
sys2=series(sysrd,sys1);
sysf=tf([50 500],[1 50 1200]);
sysout=append(1,1,sysf);
sys3=sysout*sys2
%rank(ctrb(sys3.a,sys3.b(:,2)))
%rank(obsv(sys3.a,sys3.c(2,:)))
controls=1;
feedout=2;
sys_forctr=ss(sys3.a,sys3.b(:,controls),sys3.c,sys3.d(:,controls));
[K,S,e]=lqry(sys_forctr, diag([1,1,1]),1);
[kest,L,P]=kalman(sys3,1,0.01,0,controls,feedout);
sysreg=lqgreg(kest,K,controls);
syscl=feedback(sys3,sysreg,controls,feedout);

eig(sysreg.a)
eig(syscl.a)
w=0:0.01:30*pi;
[Mag,PH]=bode(syscl,w);
A11=squeeze(Mag(1,1,:));
figure(1)
plot(w,A11,'g')
figure(2)
[Mag1,PH1]=bode(sys3,w);
A12=squeeze(Mag1(1,1,:));
plot(w,A12,'r')
figure(3)
sigma(sys3,syscl,w)

