%===================================
%parameters
M1=1.320;           %小車質(zhì)量/kg                
M2=0.208;          %1、2擺桿間的質(zhì)量快/kg 
M3=0.208;          %2、3擺桿間的質(zhì)量快/kg       
L1=0.090;          %1擺質(zhì)心到轉(zhuǎn)軸長(zhǎng)度/m         
L2=0.110;          %2擺質(zhì)心到轉(zhuǎn)軸長(zhǎng)度/m         
L3=0.270;          %3擺質(zhì)心到轉(zhuǎn)軸長(zhǎng)度/m         
c0=0;              %小車與導(dǎo)軌間的摩擦系數(shù)      
c1=0;              %1擺桿與小車間的摩擦系數(shù)     
c2=0;              %1、2擺桿間摩擦系數(shù)          
c3=0;              %2、3擺桿間摩擦系數(shù)          
m1=0.040;          %第一擺桿質(zhì)量/kg             
m2=0.050;          %第二擺桿質(zhì)量/kg             
m3=0.132;          %第三擺桿質(zhì)量/kg             
x=0;               %小車位置/m 
g=9.81;             
%state space equation
k1=m1+m2+m3+M1+M2;%
k2=m1+2*m2+2*m3+2*M1+2*M2;
k3=m1/3+m2+m3+M1+M2;
k4=m1+2*m3+2*M2;
k5=m2/3+m3+M2;
G=[k1 k2*L1 k4*L2 k3*L3;
    k2*L1 4*k3*(L1)^2 2*k4*L1*L2 2*k3*L1*L3;
    k4*L2 2*k4*L1*L2 4*k5*(L2)^2 2*k3*L2*L3;
    k3*L3 2*k3*L1*L3 2*k3*L2*L3 4/3*k3*(L3)^3];
H0=[1 0 0 0]';
H1=[-c0 0 0 0;
    0 -c1 0 0;
    0 c2 -c2 0;
    0 0  c3 -c3];
H2=[0 0 0 0;
    0 -k2*g*L1 0 0;
    0 0 -k4*g*L2 0;
    0 0 0 -k3*g*L2];
A=[zeros(4) eye(4);
    inv(G)*H2 inv(G)*H1]
B=[zeros(4,1);
    inv(G)*H0]
C=[eye(4) zeros(4)]
D=[0 0 0 0]'
op=eig(A)
Q0=1*diag([5000 100 100 100 0 0 0 0])
R0=1;
K0=lqr(A,B,Q0,R0)
K00=K0(1);
Ac0=A-B*K0;
Bc0=B*K00;
Q1 = 8*eye(4);
R1 =0.1; 
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