A=[0 0 0 1 0 0;
    0 0 0 0 1 0;
    0 0 0 0 0 1;
    0 0 0 0 0 0; 
    0 64.966 -14.865 0 0 0;
    0 -32.483 31.933 0 0 0];
B=[0;0;0;1;5.1124;0.0562];
C=eye(6);
D=[0;0;0;0;0;0];
Q0=1.6*diag([1 1 1 1 1 1]);
R0=0.8;
Q1=8*diag([1 1 1 1 1 1]);
R1=0.5;
K0=lqr(A,B,Q0,R0)
K00=K0(1);
Ac0=A-B*K0;
Bc0=B*K00;
K1=lqry(A,B,C,D,Q1,R1)
K11=K1(1);
Ac1=A-B*K1; 
Bc1=B*K11;
Cc=C;
Dc=D;
step(Ac0,Bc0,Cc,Dc)
hold on
step(Ac1,Bc1,Cc,Dc)
legend('LQR Controler','LQRY Controler')
hold off