om1=2*pi;
om2=2*pi*8;
xi1=0.025;
xi2=0.;  %0015;
%ets=0.91;
%muom=0.2;
mu=0.1;

A=[0 1 0 0; -om1^2  -2*xi1*om1  om1^2  2*xi1*om1; 0 0 0 1; ...
    om1^2/mu  2*xi1*om1/mu  -om1^2/mu-om2^2  -2*xi1*om1/mu-2*xi2*om2];
B=[0 0;  1 0; 0 0;  -1/mu  om2^2];
C=[1 0 -1 0; 0 0 1 0; -om1^2  -2*xi1*om1  om1^2  2*xi1*om1];
D=[0 0; 0 -1;   1 0];

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

sysr=tf(sqrt(0.0056*0.2*12/pi),[1 0.2*12]);
sysd=tf(1, [1/60  1]);
sysrd=append(sysd, sysr);
sysrp=series(sysrd,sys1);

sysh=append(1,1,ss([0 1;-1200 -50],[0;1],[500  50],0));
sysop=series(sysrp,sysh);

controls=1;
feedout=1;

%controlability
U=ctrb(sysop.a,sysop.b(:,controls));
rank(U)

[Ab,Bb,Cb,T,k]=ctrbf(sysop.a,sysop.b(:,controls),sysop.c);
length(Ab)-sum(k)
%pause
if sum(k)<size(Ab)
    p=1:(length(Ab)- sum(k));
    eig(Ab(p,p))
end

%observability
V=obsv(sysop.a,sysop.c(feedout,:));
rank(V);
[Ao,Bo,Co,To,ko]=obsvf(sysop.a,sysop.b,sysop.c(feedout,:));
length(Ao)-sum(ko)
%pause

%sys_in1out1=ss(sysop.a,sysop.b(:,controls),sysop.c(feedout,:),0);
%rlocus(sys_in1out1);
%[k,poles]=rlocfind(sys_in1out1);

sysop_forctr=ss(sysop.a,sysop.b(:,controls),sysop.c,sysop.d(:,controls));
[K,S,e]=lqry(sysop_forctr,diag([0.04, 0.01, 0.01]),1);

[kest,L,P]=kalman(sysop,1,0.01,0, feedout,controls);
rlqg=lqgreg(kest,K,controls);
syscl=feedback(sysop,rlqg,controls,feedout);
eig(syscl)
f=linspace(0,10*pi,1000);
%[magcl,angcl]=bode(syscl,f);
%[magop,angop]=bode(sysop,f);
magcl=freqresp(syscl,f);
magop=freqresp(sysop,f);
figure(1)
plot(f,abs(squeeze(magcl(:,1,:))),f,abs(squeeze(magop(:,1,:))))
legend('susp','road','acc','susp_{op}','road_{op}','acc_{op}')
figure(2)
sigma(syscl,'r',sysop,'b');

figure(3)
x0=zeros(16,1);
x0(1)=1;
initial(syscl,x0);

figure(4)
x0=zeros(16,1);
x0(3)=1;
initial(syscl,x0);

figure(5)
impulse(syscl)

figure(6)
step(syscl)

